%Przykładowy plik ułatwiający złożenie projektu dyplomowego inżynierskiego.
%UWAGA: Generowany napis na stronie tytułowej o treści PROJEKT DYPLOMOWY INŻYNIERSKI został zaproponowany przeze mnie i nie jest, póki co, potwierdzony przez władze wydziału. Przed ostatecznym oddaniem tak złożonej pracy należy upewnić się jaka powinna być treść tego napisu. W momencie gdy uzyskam informację na temat treści tego napisu, dokonam niezbędnych zmian w źródłach.
\documentclass[printmode]{mgr}
%opcje klasy dokumentu mgr.cls zostały opisane w dołączonej instrukcji

%poniżej deklaracje użycia pakietów, usunąć to co jest niepotrzebne
%\usepackage{polski} %przydatne podczas składania dokumentów w j. polskim
\usepackage[polish]{babel}%alternatywnie do pakietu polski, wybrać jeden z nich
%\usepackage[OT4,plmath]{polski}
\usepackage[utf8]{inputenc} %kodowanie znaków, zależne od systemu
\usepackage[T1]{fontenc} %poprawne składanie polskich czcionek

%pakiety do grafiki
\usepackage{subfigure}
\usepackage{listings}
\usepackage{graphicx}
\usepackage{epstopdf}
\usepackage{subfigure}
\usepackage{psfrag}
\usepackage{amsmath}
\usepackage{textcomp}

\lstset{ %
language=C++,                	% choose the language of the code
basicstyle=\scriptsize,       		% the size of the fonts that are used for the code
numbers=left,                   % where to put the line-numbers
numberstyle=\footnotesize,      % the size of the fonts that are used for the line-numbers
stepnumber=2,                   % the step between two line-numbers. If it's 1 each line 
                                % will be numbered
numbersep=5pt,                  % how far the line-numbers are from the code
showspaces=false,               % show spaces adding particular underscores
showstringspaces=false,         % underline spaces within strings
showtabs=false,                 % show tabs within strings adding particular underscores
frame=single,	                % adds a frame around the code
tabsize=2,	                % sets default tabsize to 2 spaces
captionpos=b,                   % sets the caption-position to bottom
breaklines=true,                % sets automatic line breaking
breakatwhitespace=false,        % sets if automatic breaks should only happen at whitespace
title=\lstname,                 % show the filename of files included with \lstinputlisting;
                                % also try caption instead of title
escapeinside={\%*}{*)},         % if you want to add a comment within your code
morekeywords={*,...}            % if you want to add more keywords to the set
}

%pakiety dodające dużo dodatkowych poleceń matematycznych
\usepackage{amsmath}
\usepackage{amsfonts}

%pakiety wspomagające i poprawiające składanie tabel
\usepackage{supertabular}
\usepackage{array}
\usepackage{tabularx}
\usepackage{hhline}

%pakiet wypisujący na marginesie etykiety równań i rysunków zdefiniowanych przez \label{}, chcąc wygenerować finalną wersję dokumentu wystarczy usunąć poniższą linię
%\usepackage{showlabels}

%definicje własnych poleceń
\newcommand{\R}{I\!\!R} %symbol liczb rzeczywistych, działa tylko w trybie matematycznym
\newtheorem{theorem}{Twierdzenie}[section] %nowe otoczenie do składania twierdzeń

%dane do złożenia strony tytułowej
\title{Podatny manipulator planarny - budowa i sterowanie}
\engtitle{Compliant planar manipulator - design and control}
\author{Michał Kot}
\supervisor{dr inż. Janusz Jakubiak, I-6}
%\guardian{dr hab. inż. Imię Nazwisko Prof. PWr, I-6} %nie używać jeśli opiekun jest tą samą osobą co prowadzący pracę

%\date{2008} %standardowo u dołu strony tytułowej umieszczany jest bieżący rok, to polecenie pozwala wstawić dowolny rok

%poniżej jest lista kierunków i specjalności na wydziale elektroniki, należy wybrać właściwe lub dopisać jeśli nie ma odpowiednich
\field{Automatyka i Robotyka (AIR)}
\specialisation{Robotyka (ARR)}

%tutaj zaczyna się właściwa treść dokumentu
\begin{document}
\bibliographystyle{plabbrv} %tylko gdy używamy BibTeXa, ustawia polski styl bibliografii

\maketitle %polecenie generujące stronę tytułową
\tableofcontents %spis treści

%poniżej znajduje się przykładowa treść dalszej części dokumentu, zainteresowanych zachęcam do rozszyfrowania frazy "Lorem ipsum" :)

\chapter{Wstęp}
Z roku na rok rośnie liczba robotów, które są w stanie zastąpić człowieka w mniej lub bardziej skomplikowanych czynnościach. Ponadto,
coraz częściej spotyka się operacje, w których ludzie i roboty współpracują ze sobą. Wiąże się to ze zwiększonymi wymogami bezpieczeństwa, 
ponieważ człowiek ze względu na delikatną budowę jest narażony na mechaniczne uszkodzenia ciała. Jednym z rozwiązań
zapewniających bezinwazyjną pracę robota jest wykorzystanie struktury równoległej i redundantnej, co pozwala kontrolować siły, którymi
robot oddziałuje na otaczające go obiekty. W związku z tym pojawienie się człowieka w położeniu kolidującym z trajektorią ruchu
robota nie spowoduje znaczącego wzrostu pojawiających się sił, a co więcej wymusi generację nowej bezkolizyjnej ścieżki jeśli to będzie możliwe.

W przypadku pojawienia się w przestrzeni roboczej małych i lekkich obiektów robot
jest w stanie je przesunąć używając nieznacznie większej siły, podczas gdy cięższe przeszkody prowadzą do modyfikacji
konfiguracji bądź też całej trasy manipulatora. Zastosowanie kontroli siły w sterowniku manipulatora umożliwia bezzwłoczne reagowanie
na niespodziewane przeszkody, ale także na celowe oddziaływanie z zadaną siłą na otoczenie. Rozwiązanie to znajduje zastosowanie
w operacjach, gdzie samo pozycjonowanie nie jest wystarczające. Do takich sytuacji należą przykładowo:
\begin{itemize}
\item chwytanie obiektów miękkich i kruchych,
\item przesuwanie obiektów,
\item rozciąganie obiektów,
\item prasowanie,
\item mycie okien.
\end{itemize}

W niniejszej pracy magisterskiej podjęta zostanie próba zaprojektowania i skonstruowania takiego robota, który
będzie w stanie nieinwazyjnie koegzystować z obiektami znajdującymi się w jego otoczeniu. Struktura kinematyczna
jest kluczowym aspektem budowania bezpiecznego modelu manipulatora, w związku z tym warto przyjrzeć się kilku
cechom, które taki robot powinien posiadać.

\section{Manipulator równoległy}
Roboty o strukturach równoległych definiuje się jako roboty, w których platforma ruchoma, tzn. sprzęg efektora, jest połączona
z podstawą -- platformą więcej niż jednym łańcuchem kinematycznym, tworząc zamknięty łańcuch kinematyczny \cite{honczarenko}.
Budowa taka ma szereg istotnych zalet w stosunku do konstrukcji konwencjonalnych -- osie napędowe manipulatora
mogą być umieszczone przy jego podstawie, dzięki czemu znacząco zredukowana jest masa części ruchomej. Umożliwia to precyzyjniejsze sterowanie,
a także uzyskiwanie większych przyspieszeń ze względu na zmniejszoną bezwładność. Ponadto siły zewnętrzne działające na efektor
nie są przenoszone przez szeregowy łańcuch elementów, lecz rozłożone na równoległe ramiona, co wpływa na dużą sztywność struktury układu.
Stosunek masy do sztywności układu ulega znaczącej poprawie, co w sposób bezpośredni przekłada się na dokładność pozycjonowania.
Przy projektowaniu manipulatora dążymy do minimalizacji jego masy, ponieważ cięższe roboty wymagają większych nakładów
energetycznych przy poruszaniu się, a także wykazują gorsze parametry bezwładnościowe. Z drugiej strony większa masa sprzyja większej sztywności,
co przekłada się na mniejsze ugięcia ogniw przy oddziaływaniu zewnętrznych sił. Ma to bezpośredni wpływ na dokładność pozycjonowania, 
ponieważ sztywniejszy układ oznacza mniejszy błąd przy przeciwdziałaniu zewnętrznej sile.

Z drugiej strony wprowadzenie równoległej struktury znacząco zwiększa złożoność równań opisujących prostą i odwrotną kinematykę, a także
dynamikę robota.
Ponadto, ograniczeniu ulega przestrzeń robocza, która może być dużo mniejsza niż gabaryty manipulatora. 
W stosunku do manipulatorów szeregowych równoległość zwiększa liczbę konfiguracji osobliwych, 
które komplikują sterowanie robotem i wymagają stosowania odpowiednich ograniczeń.
W strukturach równoległych częściej występują też potencjalne kolizje pomiędzy ramionami, a efektorem robota, co należy uwzględnić przy
definiowaniu dopuszczalnego zakresu ruchu.

Najczęściej spotyka się roboty równoległe o trzech lub nawet sześciu ramionach \cite{honczarenko}. Mimo dużej liczby napędów uzyskuje
się w nich dość niewielką liczbę stopni swobody. Ich największą zaletą jest to, że silniki i przekładnie są zamocowane
na nieruchomej podstawie (co więcej poza przestrzenią roboczą), przez co nie obciążają członów ruchomych i nie pogarszają w ten sposób
właściwości dynamicznych robota. 


\section{Manipulator redundantny} \label{sec:redundantny}
Manipulator redundantny (nadmiarowy) składa się z większej liczby przegubów niż jest ona wymagana do wykonania konkretnego zadania. 
Cecha ta zapewnia to robotowi większą zręczność, która pozwala na skuteczne omijanie osobliwości, ograniczeń przegubów czy
przeszkód pojawiających się w przestrzeni roboczej \cite{handbook}. Wpływa to korzystnie na rozmiar samej przestrzeni roboczej,
ponieważ liczba osiągalnych konfiguracji ulega zwiększeniu. Co więcej, zdolność realizacji pojedynczej konfiguracji na wiele sposobów
umożliwia stosowanie optymalizacji dla tego samego położenia w przestrzeni zewnętrznej w oparciu o przyjęte kryteria. Pojedyncze
konfiguracje składają się na całe trajektorie, w związku z czym możliwa jest optymalizacja ścieżki i trajektorii robota.
Wiąże się to ze zmniejszonymi momentami i energią w przegubach,
co wpływa na poprawę dynamicznych osiągów robota. Dla manipulatorów redundantnych komplikacji ulega zadanie odwrotnej kinematyki, 
ponieważ istnieje nieskończona liczba jej rozwiązań. W związku z tym definiuje się kryteria doboru konfiguracji, 
tak aby obliczone położenia przegubów były optymalne. 


\section{Manipulator planarny}
Planarność, jako cecha manipulatora, oznacza, że porusza się on jedynie na płaszczyźnie, a nie w przestrzeni trójwymiarowej.
Położenie efektora opisuje się za pomocą współrzędnych położenia X i Y oraz orientacji $\theta$. Takie ograniczenie znacząco redukuje 
stopień złożoności ruchu robota, a w konsekwencji równania opisujące jego ruch. Przy konstruowaniu manipulatorów
planarnych istotna jest redukcja masy ruchomych elementów. Dobranie odpowiedniej orientacji robota w przestrzeni może
(niecałkowicie) wyeliminować wpływ przyciągania ziemskiego na dynamikę, ponieważ napędy będą poruszać elementami prostopadle do siły grawitacji
(obciążenie rozłoży się na napędy i łożyska).

\section{Manipulator podatny}\label{sec:podatnosc}
Podatność \cite{handbook} definiowana jest jako zdolność robota do reagowania na siły, którymi oddziałuje on na otoczenie,
bez znajomości precyzyjnego modelu środowiska, w którym się znajduje. W tradycyjnym podejściu, 
do sterowania ruchu manipulatora wykorzystuje się jedynie położenia i prędkości.
Takie rozwiązanie nie jest odporne na potencjalne siły mogące się pojawić przy kontakcie z niespodziewanymi obiektami, 
występujące na skutek zakłóceń powodowanych przez siły zewnętrzne. Obserwowane
w tym momencie przyspieszenia prowadzą do sporych rozbieżności i błędów pozycjonowania efektora. Problem ten może być
rozwiązany poprzez wprowadzenie równoległego sterowania siłą, które umożliwia regulację wartości oddziaływania efektora
na otoczenie. Kontrolowanie siły (oddziaływania) efektora (na otoczenie) znajduje swoje zastosowanie zarówno 
przy zatrzymywaniu ruchu robota w kontakcie z
obiektami sztywnymi, jak i zręcznym manipulowaniu obiektami miękkimi (np. chwytanie szklanki lub gąbki).

Istnieje cały szereg metod stosowanych do sterowania siłowego. Jedną z nich jest podatność pasywna, która wymaga
szczególnej budowy manipulatora -- podatność realizowana jest poprzez konstrukcję mechaniczną, która pod wpływem zewnętrznych
oddziaływań zmienia swój kształt (np. zgina się) amortyzując pojawiające się siły. 
Zapewnienie strukturalnej podatności odbywa się za pomocą podatnych bądź sprężystych ogniw, przegubów, efektora
czy też podatnych serwomechanizmów, służących do pozycjonowania. Jednym z przykładów jest zastosowanie
równoległej budowy robota. Podatność pasywna charakteryzuje się dużą prostotą (realizacji) i niskim nakładem kosztów, ze względu na
brak stosowania elektroniki -- czujników i sterowników. Mimo wszystko jest to rozwiązanie mało efektywne -- niemożliwa jest
szybka rekonfiguracja manipulatora. Ponadto, radzi sobie ono jedynie z niewielkimi niedokładnościami orientacji i pozycjonowania robota.
Przy większych zaburzeniach nie jesteśmy w stanie jednoznacznie stwierdzić czy pojawiły się duże siły zewnętrzne.

Podatność aktywna manipulatora wynika najczęściej z zastosowanego układu sterowania. Wykorzystując sygnały z
różnego rodzaju czujników sterownik otrzymuje informacje na temat sił oddziałujących z zewnątrz na manipulator, dzięki
czemu może on modyfikować trajektorię w czasie rzeczywistym. Ten rodzaj podatności jest bardziej skomplikowany,
co implikuje większe koszta i wolniejsze działanie. Z drugiej strony aktywna podatność oferuje pełną kontrolę nad
realizowaną trajektorią, co zwiększa dokładność i powtarzalność sterowania. Ze względu na niedostatecznie szybką reakcję
na zaburzenia, często do aktywnej podatności dodaje się elementy podatności pasywnej.

Strategie aktywnego kontrolowania interakcji dzieli się na dwie kategorie -- pośrednie i bezpośrednie.
Pierwsza z nich, do której należy kontrola impedancji, nie opiera się na bezpośrednim sprzężeniu zwrotnym od siły, a na analizie ruchu
i reakcjach robota na zaburzenia w zaplanowanej trajektorii. Kontrolowanie bezpośrednie opiera się na pełnym modelu zadania interakcji
i wymaga pełnej informacji zwrotnej. Popularnym i często stosowanym pośrednim podejściem do sterowania manipulatora w kontakcie z nie w 
pełni znanym środowiskiem jest wykorzystanie hybrydowego połączenia sterowania przyspieszeniem i siłą. 
Kontrolowanie samej siły nie zapewnia odpowiednich właściwości dynamicznych ruchu
robota, podczas gdy sterowanie przyspieszeniem w kontakcie z przeszkodami prowadzi do błędów wspomnianych wcześniej. 
O ile zastosowanie obu sterowań równolegle jest dość skomplikowane, stosuje się pewne uproszczenia pozwalające zdekomponować
problem na dwa części. Możliwe jest rozdzielenie stopni swobody robota na pracę w domenie sił i przyspieszeń w taki sposób,
aby nie zachodziły one na siebie. W rezultacie, otrzymuje się manipulator sterujący siłą/momentem w wybranych kierunkach,
a położeniem w pozostałych.


\chapter{Cele projektu} \label{ch:cele}
Celem projektu zrealizowanego w ramach niniejszej pracy magisterskiej jest zaprojektowanie i skonstruowanie planarnego, równoległego
i redundantnego manipulatora, który będzie w stanie analizować siły działające na niego z zewnątrz. Umożliwi to 
zręczne poruszanie się robota w środowisku, ponieważ każda napotkana przeszkoda spowoduje zatrzymanie go bądź też zmusi
do osiągnięcia zadanego położenia w innej konfiguracji przegubów. Dodatkową cechą, która zostanie zaimplementowana w manipulatorze
jest możliwość uczenia się ruchów zadanych manualnie przez operatora -- konkretna ścieżka może zostać zapisana na podstawie
odczytów sił działających na napędy. 

\section{Aspekt inżynierski}
Aspekt inżynierski projektu zakłada zaprojektowanie mechaniki i układu sterowania manipulatora. Jest on ściśle powiązany
z aspektem badawczym (\ref{sec:aspekt_badawczy}), ponieważ dobór parametrów układu mechanicznego musiał zostać poprzedzony
szeregiem analiz i symulacji. Trójwymiarowy model obiektu został wykonany z wykorzystaniem środowiska \texttt{Autodesk Inventor} \cite{autodesk},
które to umożliwia szybkie i wygodnie modelowanie złożonych konstrukcji mechanicznych. Co więcej, posiada on plugin
\texttt{SimMechanics Link} \cite{simmechanics_link}, umożliwiający eksport modelu do toolboxa \texttt{Simulink} środowiska 
\texttt{Matlab} \cite{mathworks}, które jest jednym z najpopularniejszych i najbardziej rozbudowanych aplikacji symulacyjnych.
W trakcie prac z wykorzystaniem Inventora skonstruowane zostały cztery podobne modele manipulatora, które ewoluowały 
w kierunku wersji docelowej. 

Projektowanie i implementacja algorytmów sterowania ..... Tu będzie dalszy tekst, gdy zacznę prace nad tym etapem.

\section{Aspekt badawczy}\label{sec:aspekt_badawczy}
Opracowanie kryteriów doboru parametrów kinematycznych manipulatora stanowiło jeden z kluczowych elementów aspektu badawczego projektu.
Proporcje długości ramion robota mają istotny wpływ na jakość i wielofunkcyjność jego pracy, w związku z czym definiuje się cały
szereg kryteriów \cite{miary_jakosci}, które różnią się pomiędzy sobą zarówno podejściem, jak i stopniem skomplikowania:
\begin{itemize}
\item ekscentryczność manipulatora, mierząca odległość położenia przegubów od ich pozycji środkowych,
\item manipulowalność manipulatora, która jest miarą wrażliwości efektora na lokalne wariacje konfiguracji,
\item współczynnik uwarunkowania, będący miarą stopnia anizotropowości konfiguracji,
\item dystorsja kinematyczna, jako miara niesztywności kinematyki,
\item objętość przestrzeni roboczej,
\item gęstość objętości kinematyki.
\end{itemize}
W teorii robotyki stosuje się także kombinacje kilku różnych kryteriów. W przypadku niniejszego projektu zastosowane zostało
kryterium mówiące o objętości przestrzeni roboczej, co w przypadku manipulatorów planarnych sprowadza się do powierzchni przestrzeni roboczej. 
Obliczenie przestrzeni z dużą dokładnością dla wielu różnych konfiguracji robota jest niemożliwe bez wykorzystania komputera. W związku
z tym konieczne okazało się napisanie programu z wykorzystaniem języka C++, który wyszukuje optymalną konfigurację parametrów manipulatora. 
Szerzej zostało to opisane w rozdziale \ref{sec:wyznaczenie_wymiarow_manipulatora}.

Istotną częścią aspektu badawczego projektu były także badania porównawcze algorytmów sterowania oraz ocena ich własności.
.....
Tu będzie dalszy tekst, gdy zacznę prace nad tym etapem.


\chapter{Pierwowzór} \label{ch:pierwowzor}
Pomysł na stworzenie planarnego manipulatora wywodzi się z \cite{wzor}. 
Autorzy tego artykułu podjęli się stworzenia równoległego manipulatora, którego cechą charakterystyczną
jest elastyczna końcówka umożliwiająca pomiary sił/momentów działających na efektor. 
Jak widzimy na rysunku \ref{rys:pierwowzor_zdjecie}, do pomiaru siły został wykorzystany czujnik sił/momentów
o sześciu stopniach swobody, dzięki czemu możliwe jest wykrycie każdego rodzaju deformacji elastycznej
końcówki. Zaletą takiego rozwiązania jest zdolność do analizy sił i momentów, którymi otoczenie
oddziałuje na efektor. W rezultacie, poprzez zastosowanie hybrydowego sterowania, polegającego
na jednoczesnym kontrolowaniu położenia efektora i sił na niego oddziałujących, jesteśmy w stanie zapewnić
dokładniejszą i bezpieczniejszą interakcję manipulatora z otoczeniem. Wynika to z braku dużych sił powstających
przy wykorzystaniu jedynie kontroli położenia.
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/pierwowzor_zdjecie}
  \caption{Pierwowzór konstruowanego manipulatora}
  \label{rys:pierwowzor_zdjecie}  
\end{figure}

\section{Różnice w stosunku do pierwowzoru}\label{sec:roznice_do_pierwowzoru}
Stworzony w ramach niniejszej pracy magisterskiej manipulator różni się jednakże w wielu aspektach od
swojego pierwowzoru. Na rysunku \ref{rys:pierwowzor_model} znajduje się model pierwowzoru, jednakże po szeregu badań i analiz 
okazało się, że konstrukcja tamtego manipulatora nie zakładała redundancji, która jest jednym z
fundamentów tego projektu. Redundancja, która umożliwia osiągnięcie jednego położenia efektora przy pomocy
wielu różnych konfiguracji manipulatora, została zapewniona poprzez wprowadzenie jednego dodatkowego przegubu
pasywnego. Co więcej, w stosunku do pierwowzoru elastyczne ramie połączone z czujnikiem sił i momentów
zostało zastąpione przez inny mechanizm, opisany dokładniej w rozdziale \ref{sec:czujniki_sily}.
Dzięki zmianom w konstrukcji manipulator stworzony w ramach tego projektu posiada większą gamę
potencjalnych zastosowań, np. możliwość uczenia się. 

\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/pierwowzor_model}
  \caption{Model pierwowzoru konstruowanego manipulatora}
  \label{rys:pierwowzor_model}  
\end{figure}


\section{Inne projekty wykorzystujące podobne rozwiązania}
Przed przystąpieniem do projektowania manipulatora przeanalizowane zostały podobne rozwiązania, które
dostarczyły pomysłów dla realizacji projektu. Praca \cite{inne1} przedstawia konstrukcję podatnego manipulatora planarnego,
który ma możliwość operowania i interakcji z obiektami, które nie są precyzyjnie umieszczone w jego otoczeniu.
Zastosowane tam zostały innowacyjne algorytmy sterowania pozwalające kontrolować sztywność i impedancję manipulatora.
Dzięki sprężynom umieszczonym szeregowo z ramionami robot kontroluje siły oddziałujące na niego z otoczenia. 
W samej pracy znajduje się szereg badań i analiz, które przedstawiają wydajność takiego rozwiązania.

W artykule \cite{inne2} znajduje się inny algorytm sterowania podatnego manipulatora planarnego, oparty na
strategii AFC (active force control -- aktywne sterowanie siłą). Podejście to w połączeniu z regulatorem PID
w skuteczny sposób redukuje zaburzenia pozycjonowania ramion robota. Całość została przetestowana na równoległym manipulatorze typu RRR
o trzech stopniach swobody, dla którego w raporcie wyprowadzone zostały prosta i odwrotna kinematyka.

Interesujący robot o dwóch stopniach swobody znajduje się w artykule \cite{inne3}. Jest to planarny, równoległy manipulator 
wykorzystywany przy pracy z systemami półprzewodnikowymi. Ze względu na minimalizację masy poruszających się elementów
robot pozycjonowany jest z niezwykle dużą dokładnością, co jest wymagane przy pracy z półprzewodnikami. Zwiększenie precyzji
pozycjonowania efektora na płaszczyźnie XY otrzymano poprzez zastosowanie równoległej budowy robota. W raporcie tym znajduje
się także bogate porównanie z tradycyjnymi metodami sterowania, uwypuklające wady i zalety mechanizmów równoległych.

Dynamikę i sterowanie manipulatora redundantnego można znaleźć w pracy \cite{inne4}. Bazując na modelu dynamiki przedstawione
są strategie sterowania pozycją i siłą, stosowane naprzemiennie. Zdefiniowane są krytyczne kąty przegubów, przy których następuje zmiana
strategii. W raporcie tym znajdują się badania porównujące jakość pozycjonowania efektora dla redundantnej i nieredundantnej wersji manipulatora.
Na podstawie przeprowadzonych eksperymentów autorzy stwierdzają, że błędy w obu przypadkach są do siebie zbliżone. Istotnym wnioskiem jest
fakt, że wprowadzenie redundacji nie wpływa na jakość pozycjonowania, a jednocześnie zwiększa sztywność i możliwości manipulacyjne robota.

\section{Testowe modele manipulatora}
Przed pojawieniem się finalnej wersji manipulatora rozważane były różne warianty, a projekt ewoluował od pierwowzoru \ref{rys:pierwowzor_model}
do wersji finalnej \ref{rys:manipulator_docelowy_model}. Wersja oparta na pierwowzorze została odrzucona po pierwszych
testach, głównie ze względu na wady opisane w sekcji \ref{sec:roznice_do_pierwowzoru}. 
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/testowa_wersja_model}
  \caption{Testowy model manipulatora}
  \label{rys:testowa_wersja_model}  
\end{figure}

Model kolejnego rozpatrywanego wariantu znajduje się na rysunku \ref{rys:testowa_wersja_model}. W stosunku do pierwowzoru pojawił
się jeden dodatkowy przegub, który zapewnił redundancję manipulatora. Ponadto efektorem jest sztywna część (w pierwowzorze była elastyczna),
co całkowicie zmienia sposób sterowania i wykorzystania czujników siły. 

\subsection{Prosta kinematyka manipulatora}
Wyznaczenie kinematyki prostej, polegające
na obliczaniu pozycji efektora ($x_e, y_e, \theta_e$) na podstawie położeń przegubów ($q_{a1}, q_{a2}, q_{b1}, q_{b2}$)
zostało oparte o rysunek \ref{rys:schemat_probny}. Punkty $(x_a, y_a)$ oraz $(x_b, y_b)$ reprezentują zakończenia pierwszych
dwóch ramion (odpowiednio prawego i lewego), tworzących dwuwahadło. Wyprowadzenie prostej kinematyki zawierają równania 
\ref{eq:testowa_kinematyka_prosta2} -- \ref{eq:testowa_kinematyka_prosta}.
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/schemat_probny}
  \caption{Schemat próbnego manipulatora}
  \label{rys:schemat_probny}  
\end{figure}

Przy liczeniu kinematyki tego manipulatora należy wziąć pod uwagę kilka założeń dotyczących modelu -- takich jak stały kąt $\beta$.
Wynika on ze sztywnego połączenia ogniwa efektora z ostatnim ogniwem lewego ramienia, a jego wartość została ustalona na $\beta = \frac{2}{3}\pi$.

Na podstawie kątów przegubów napędzanych, niezależnie dla lewego i prawego ramienia obliczane są położenia końców drugich ramion
($l_0$ jest odległością początków każdego z ramion od punktu (0,0)):
\begin{equation}
\begin{cases}
x_a = l_0 + l_1\cos q_{a1} + l_2 \cos(q_{a1}+q_{a2}),\\
x_b = -l_0 + l_1\cos q_{b1} + l_2 \cos(q_{b1}+q_{b2}),\\
y_a = l_1\sin q_{a1} + l_2 \sin(q_{a1}+q_{a2}),\\
y_b = l_1\sin q_{b1} + l_2 \sin(q_{b1}+q_{b2}).
\end{cases}
\label{eq:testowa_kinematyka_prosta2}
\end{equation}

Korzystając z twierdzenia cosinusów wyznaczamy kąt $\delta$, a w konsekwencji punkt łączenia ramion $(x_s, y_s)$:
\begin{equation}
\begin{cases}
\alpha = \arctan(\frac{y_a-y_b}{x_a-x_b}),\\
\delta = \arccos(\frac{d}{2l_3}),\\
x_s = x_b + l_3\cos(\delta + \alpha),\\
y_s = y_b + l_3\sin(\delta + \alpha).
\end{cases}
\label{eq:testowa_kinematyka_prosta3}
\end{equation}

W oparciu o ten punkt, a także kąty $\beta$ i $\delta$ wyznaczamy położenie i orientację efektora, które zawiera
równanie (\ref{eq:testowa_kinematyka_prosta}).

\begin{equation}
\begin{cases}
x_e = x_s + l_4\cos\theta_e\\
y_e = y_s + l_4\sin\theta_e,\\
\theta_e = \frac{3}{2}\pi - \beta - \varphi = \pi - \beta + \delta + \alpha.
\end{cases}
\label{eq:testowa_kinematyka_prosta}
\end{equation}

\subsection{Odwrotna kinematyka manipulatora}
Zadaniem kinematyki odwrotnej jest wyznaczenie położeń przegubów ($q_{a1}, q_{a2}, q_{b1}, q_{b2}$) przy znanej pozycji efektora
($x_e, y_e, \theta_e$). Równania \ref{eq:testowa_kinematyka_odwrotna2} -- \ref{eq:testowa_kinematyka_odwrotna} zawierają te przekształcenia.

Parametr $\gamma$ opisuje kąt pomiędzy ostatnim ogniwem prawego ramienia, a ogniwem efektora. Ze względu na redundantny charakter manipulatora
zadanie odwrotnej kinematyki posiada nieskończoną liczbę rozwiązań. W związku z tym konieczne jest wprowadzenie dodatkowego parametru.
Kąt $\gamma$ podlega optymalizacji w trakcie procesu liczenia odwrotnej kinematyki, gdyż miał on służyć do kontrolowania równomierności
rozkładu sił działających na efektor na obydwa ramiona. Ze względu na pominięcie analizy sił dla testowej wersji manipulatora, można
przyjąć go jako wartość stałą.

Odwrotną kinematykę wyznacza się zaczynając od końca i postępując wstecz. W pierwszym etapie, na podstawie sinusa i cosinusa kąta
$\theta_e$ oraz punktu $(x_e, y_e)$ obliczane jest położenie punktu łączącego oba ramiona $(x_s, y_s)$. 
\begin{equation}
\begin{cases}
x_s = x_e - l_4\cos(\theta_e),\\
y_s = y_e - l_4\sin(\theta_e).
\end{cases}
\label{eq:testowa_kinematyka_odwrotna2}
\end{equation}

W kroku kolejnym, 
wyznaczany jest kąt $\phi$, a także nachylenie prawego ogniwa $l_3$ w globalnym
układzie współrzędnych $\eta$ jako różnicę $\gamma - \theta_e$. 
\begin{equation}
\begin{cases}
\varphi = \frac{3}{2}\pi - \beta - \theta_e,\\
\eta = \gamma - \theta_e.
\end{cases}
\label{eq:testowa_kinematyka_odwrotna3}
\end{equation}

Na podstawie tych dwóch kątów, a także położenia punktu złączenia ramion
otrzymywane są położenia $(x_a, y_a)$ i $(x_b, y_b)$:
\begin{equation}
\begin{cases}
x_a = x_s + l_3\cos(\eta),\\
y_a = y_s - l_3\sin(\eta),\\
x_b = x_s - l_3\sin(\phi),\\
y_b = y_s - l_3\cos(\phi).
\end{cases}
\label{eq:testowa_kinematyka_odwrotna4}
\end{equation}

Kąty napędów ($q_{a1}, q_{a2}$) oraz ($q_{b1}, q_{b2}$)
wyznaczane są niezależnie według standardowych procedur liczenia kinematyki odwrotnej dwuwahadła, uwzględniając odcinek $l_0$.

\begin{equation}
\begin{cases}
q_{a1} = \arctan(\frac{y_a}{x_a-l_0})-\arccos(\frac{l_1^2-l_2^2+(x_a-l_0)^2+y_a^2}{2l_1\sqrt{(x_a-l_0)^2+y_a^2}}),\\
q_{a2} = \pi - \arccos(\frac{l_1^2+l_2^2-(x_a-l_0)^2-y_a^2}{2l_1l_2}),\\
q_{b1} = \arctan(\frac{y_b}{x_b+l_0}) + \arccos(\frac{l_1^2-l_2^2+(x_b+l_0)^2+y_b^2}{2l_1\sqrt{(x_b+l_0)^2+y_b^2}}),\\
q_{b2} = -\pi + \arccos(\frac{l_1^2+l_2^2-(x_b+l_0)^2-y_b^2}{2l_1l_2}),
\end{cases}
\label{eq:testowa_kinematyka_odwrotna}
\end{equation}

Dla takiego manipulatora zostały przeprowadzone badania wielkości jego przestrzeni roboczej, jej kształtu i wpływu na rozmiar robota.
Znajdują się one w rozdziale \ref{sec:wymiary_probnej_wersji}. 
Sztywne połączenie pomiędzy efektorem, a jednym z ramion sprawia, że manipulator nie jest symetryczny względem ramion. 
Prowadzi to do nierównomiernego rozkładu sił działających na efektor w obu ramionach robota (jego przegubach i silnikach).
Symetryczny rozkład sił umożliwia precyzyjniejszy ich pomiar, co jest jednym z założeń projektu. 
W związku z tym ten wariant projektu został odrzucony, a w kolejnej wersji manipulator został wzbogacony o opisywaną symetrię. 
Ponadto wyeliminowanie wspomnianego sztywnego połączenia zwiększyło powierzchnię jego przestrzeni roboczej, co bezpośrednio przełożyło
się na poprawę możliwości manipulacyjnych.


\chapter{Struktura manipulatora docelowego}\label{sec:struktura_manipulatora_docelowego}
Na rysunku \ref{rys:docelowy_model} znajduje się stworzony w programie \texttt{Autodesk Inventor} model
ostatecznej wersji manipulatora. Jego struktura odpowiada strukturze robota fizycznie skonstruowanego, z dokładnością
do poszczególnych części, które zostały użyte do realizacji poszczególnych elementów. Właściwości i parametry mechaniczne
konstrukcji zostaną opisane w rozdziale \ref{sec:fizyczna_realizacja} przedstawiającym fizyczną realizację manipulatora.
W tym rozdziale nacisk zostanie położony na kinematyczny aspekt budowy robota.
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/docelowy_model}
  \caption{Model konstruowanego manipulatora}
  \label{rys:docelowy_model}  
\end{figure}

\section{Konstrukcja manipulatora}
Konstrukcja manipulatora rozpoczyna się od czterech napędów, umieszczonych po lewej stronie na rysunku \ref{rys:docelowy_model}.
Wszystkie pozostałe przeguby są nienapędzane (pasywne). Każdym z dwóch ramion sterują dwa silniki. 
Mechanizm równoległy umieszczony na początku każdego z ramion
pozwala uzyskać możliwość manipulowania kolejnym ogniwem bez wprowadzania dodatkowego napędu, dzięki czemu
zmniejszona jest masa i bezwładność poruszającego się ramienia.
Zastosowane na końcu połączenie belki efektora z każdym z ramion w dwóch miejscach zapewnia symetryczne rozłożenie
sił działających na efektor względem obu ramion. Zastosowanie mechanizmu przesuwnego pozwala
na swobodny ruch manipulatora, przy zachowaniu równej odległości pomiędzy belką efektora i ramionami.

\section{Ograniczenia ruchu robota}\label{sec:konfiguracje_niedozwolone}
Równoległa struktura manipulatora niesie za sobą szereg ograniczeń na konfiguracje przegubów, ponieważ
istnieją potencjalne, wzajemne kolizje ramion, a także ramion i efektora. Konieczne jest wprowadzenie odpowiednich restrykcji
na zakresy kątów napędów w celu uniknięcia uszkodzeń mechanicznych robota. Mechanizm
łączący dwa silniki definiuje ograniczenie ich obrotu ze względu możliwą kolizję dwóch równoległych ogniw. 
Ponadto uszkodzenia mogą pojawić się w konfiguracjach skrajnych, w których to jedno bądź oba ramiona są mocno
skierowane ku sobie. Mogłoby to doprowadzić do zetknięcia się efektora z ramieniem bądź też ramion pomiędzy sobą.
Jednym ze sposobów zarządzania ograniczeniami jest zapewnienie wypukłości zamkniętego łańcucha manipulatora, 
które poza wyeliminowaniem potencjalnych kolizji, zapewni także również prawidłowy rozkład sił.

W manipulatorach równoległych pojawiają się także ograniczenie kątów wynikające z kinematyki 
(rozdział \ref{sec:kinematyka_manipulatora} i równanie (\ref{rys:schemat_docelowy})). Wartości kątów $q_{a1}$ i $q_{b1}$ przy pełnej swobodzie obrotu
mogłyby skierować ramiona $l_1$ poniżej osi X, co skutkowałoby zwróceniem się efektora w przeciwną stronę i licznymi kolizjami.
Ponadto, kąty przegubów muszą być tak dobrane, aby manipulator tworzył zamknięty łańcuch -- można bez trudu wyobrazić sobie sytuację,
w której odległość punktów $(x_a, y_a)$ oraz $(x_b, y_b)$ jest większa od $2l_3$. W związku z tym ograniczenia na kąty napędów
muszą być zdefiniowane zależnie względem siebie. Kolejne ograniczenie na położenie końców ramion sterowanych opisuje nierówność $x_b < x_a$.
Wynika ono zarówno z równania kinematyki (\ref{eq:kinematyka_prosta2}), 
jak i potrzeby eliminacji licznych kolizji, które mogą się pojawić przy naruszeniu tego warunku.

Dodatkowe ograniczenia pojawiają się przy rozpatrywaniu dynamiki robota. Maksymalne prędkości obrotowe silników, a także
bezwładności wynikające z niezerowej masy ogniw robota prowadzą do ograniczenia przestrzeni dopuszczalnych prędkości i przyspieszeń manipulatora.
Ponadto, zastosowanie przekładni do napędów redukuje tę przestrzeń, ponieważ większy nominalny moment obrotowy niesie za sobą
mniejszą maksymalną prędkość. Konkretne ograniczenia na wartości kątów zostaną szerzej opisane w części dotyczącej implementacji, 
znajdującej się w rozdziale \ref{sec:ograniczenia_katow}. 


\section{Kinematyka manipulatora}\label{sec:kinematyka_manipulatora}
Efektorem (chwytakiem) nazywa się zakończenie konstrukcji manipulatora, które często posiada możliwość wymiany bądź modyfikacji. 
Dzięki temu jeden robot może sekwencyjnie wykonywać kilka różniących się od siebie operacji. Pod pojęciem kinematyki kryje się
funkcja odwzorowująca przestrzeń stanu \emph{Q}, zawierającą położenia napędów $q \in Q$,
w przestrzeń \emph{P} zawierającą wektor położenia i orientacji efektora $p \in P$ ($R^n$, SO(3) lub inne) \cite{podstawy_robotyki}.
Znajomość kinematyki jest niezbędna do manipulowania robotem, ponieważ na jej podstawie wyznaczane jest
aktualne położenie efektora. Odwrotna kinematyka \ref{sec:odwrotna_kinematyka} pozwala zaś na obliczenie konkretnej
konfiguracji przegubów (położeń napędów) dla zadanej pozycji efektora, co jest wykorzystywane przy planowaniu trasy robota w
przestrzeni zadaniowej. Algorytmy sterowania wykorzystują kinematykę w celu przewidzenia zmian, które nastąpią po aktualizacji pozycji napędów.

\subsection{Prosta kinematyka manipulatora}\label{sec:prosta_kinematyka}
Kinematyka prosta jest to przekształcenie geometryczne, transformujące konfigurację z przestrzeni stanu 
$q = (q_{a1}, q_{a2}, q_{b1}, q_{b2}) \in Q$ w wektor w przestrzeni
roboczej $p = (x_e, y_e, \theta_e) \in P$.
Ze względu na planarność konstruowanego manipulatora wyznaczenie kinematyki sprowadza się do obliczenia położeń $x_e$ i $y_e$ oraz
orientacji $\theta_e$, gdyż
ruch robota odbywa się na płaszczyźnie i wartość współrzędnej Z (prostopadłej do płaszczyzny XY) jest stała.

\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/schemat_docelowy}
  \caption{Schemat docelowego manipulatora}
  \label{rys:schemat_docelowy}  
\end{figure}

W wyniku wprowadzenia symetryczności w stosunku do kinematyki testowej wersji robota
uproszczeniu uległy równania \ref{eq:kinematyka_prosta1} -- \ref{eq:kinematyka_prosta3} 
zawierające prostą kinematykę zaprojektowanego manipulatora.
Wykorzystane oznaczenia znajdują swoją reprezentację na schemacie \ref{rys:schemat_docelowy}.

Wyprowadzenie kinematyki prostej ponownie rozpoczyna się od wyznaczenia punktów końca ramion bezpośrednio napędzanych 
$(x_a, y_a)$ oraz $(x_b, y_b)$ na podstawie
sinusów i cosinusów przegubów $q \in Q$, a także odległości $l_0$:
\begin{equation}
\begin{cases}
x_a = L + l_1\cos q_{a1} + l_2 \cos(q_{a1}+q_{a2}),\\
x_b = -L + l_1\cos q_{b1} + l_2 \cos(q_{b1}+q_{b2}),\\
y_a = l_1\sin q_{a1} + l_2 \sin(q_{a1}+q_{a2}),\\
y_b = l_1\sin q_{b1} + l_2 \sin(q_{b1}+q_{b2}).
\end{cases}
\label{eq:kinematyka_prosta1}
\end{equation}

Korzystając z twierdzenia cosinusów wyznaczamy kąt $\delta$, a w konsekwencji punkt łączenia ramion $(x_s, y_s)$:
\begin{equation}
\begin{cases}
\alpha = \arctan(\frac{y_a-y_b}{x_a-x_b}),\\
\delta = \arccos(\frac{d}{2l_3}),\\
x_s = x_b + l_3\cos(\delta + \alpha),\\
y_s = y_b + l_3\sin(\delta + \alpha).
\end{cases}
\label{eq:kinematyka_prosta2}
\end{equation}

W układzie równań \ref{eq:kinematyka_prosta2} zakładamy, że $x_a \neq x_b$, zgodnie z ograniczeniami opisanymi 
w rozdziale \ref{sec:konfiguracje_niedozwolone}.
Orientacja manipulatora $\theta_e$ definiowana jest jako kąt $\alpha$, jednoznacznie określony przez położenia $(x_a, y_a)$ i $(x_b, y_b)$,
powiększony o $\frac{\pi}{2}$. Z wykorzystaniem orientacji $\theta_e$, punktu łączenia ramion $(x_s, y_s)$, 
a także ogniwa $l_4$ wyznaczane jest położenie efektora $(x_e, y_e)$:

\begin{equation}
\begin{cases}
x_e = x_s + l_4\cos\theta_e\\
y_e = y_s + l_4\sin\theta_e,\\
\theta_e =  \alpha + \frac{\pi}{2}.
\end{cases}
\label{eq:kinematyka_prosta3}
\end{equation}

\subsection{Odwrotna kinematyka manipulatora}\label{sec:odwrotna_kinematyka}
Odwrotna kinematyka służy do wyznaczania wartości kątów w napędach robota ($q \in Q$) na podstawie informacji o 
położeniu i orientacji efektora $p \in P$. Jest to zadanie bardziej złożone niż kinematyka prosta, które w skrajnych przypadkach
może nie mieć rozwiązania (w momencie, gdy zadane położenie efektora jest nieosiągalne bez naruszenia ograniczeń). 
Ponadto, nawet jeśli istnieje rozwiązanie odwrotnej kinematyki, konfiguracja może być mimo wszystko nieosiągalna dla manipulatora
-- konfiguracje pośrednie, teoretycznie umożliwiające dotarcie do niej mogą naruszać zadane ograniczenia.

Redundantna struktura robota dodatkowo komplikuje zadanie odwrotnej kinematyki, ponieważ z definicji \ref{sec:redundantny}
manipulator redundantny może osiągnąć konfigurację na nieskończenie wiele sposobów. Implikuje to nieskończoną
liczbę rozwiązań zadania odwrotnej kinematyki. W związku z tym wprowadza się dodatkowy parametr (kryterium), który
wykorzystuje się do wyboru sposobu optymalizacji konfiguracji wynikowej. Możliwe jest stosowanie lokalnych miar jakości, które są szczególnymi
przypadkami kryteriów przedstawionych w rozdziale \ref{sec:aspekt_badawczy}. 

Wyznaczanie odwrotnej kinematyki rozpoczyna się od obliczenia położenia punktu łączącego oba ramiona $(x_s, y_s)$:
\begin{equation}
\begin{cases}
x_s = x_e + l_4\cos(\theta_e),\\
y_s = y_e - l_4\sin(\theta_e).
\end{cases}
\label{eq:kinematyka_odwrotna2}
\end{equation}

Przy analizie projektowanego manipulatora dodatkowym parametrem jest kąt $\gamma$, określony pomiędzy ogniwem efektora, 
a ostatnimi ogniwami każdego z ramion.
Jest on odpowiedzialny za rozkład siły działającej na efektor na obydwa ramiona, w związku z czym jest to istotny aspekt analizy sił.

Korzystając z orientacji manipulatora, parametru $\gamma$ oraz punktu $(x_s, y_s)$ wyznaczamy położenia punktów $(x_a, y_a)$ oraz $(x_b, y_b)$:
\begin{equation}
\begin{cases}
\alpha = \theta_e - \frac{\pi}{2}, \\
x_a = x_s + l_3\sin(\gamma + \alpha),\\
y_a = y_s - l_3\cos(\gamma + \alpha),\\
x_b = x_s - l_3\sin(\gamma - \alpha),\\
y_b = y_s - l_3\cos(\gamma - \alpha).
\end{cases}
\label{eq:kinematyka_odwrotna3}
\end{equation}

Kąty napędów ($q_{a1}, q_{a2}$) oraz ($q_{b1}, q_{b2}$)
wyznaczane są niezależnie według standardowych procedur liczenia kinematyki odwrotnej dwuwahadła, uwzględniając przesunięcie pierwszych
przegubów ramion o $(\pm l_0, 0)$, identycznie jak dla manipulatora testowego:
\begin{equation}
\begin{cases}
q_{a1} = \arctan(\frac{y_a}{x_a-l_0})-\arccos(\frac{l_1^2-l_2^2+(x_a-l_0)^2+y_a^2}{2l_1\sqrt{(x_a-l_0)^2+y_a^2}}),\\
q_{a2} = \pi - \arccos(\frac{l_1^2+l_2^2-(x_a-l_0)^2-y_a^2}{2l_1l_2}),\\
q_{b1} = \arctan(\frac{y_b}{x_b+l_0}) + \arccos(\frac{l_1^2-l_2^2+(x_b+l_0)^2+y_b^2}{2l_1\sqrt{(x_b+l_0)^2+y_b^2}}),\\
q_{b2} = -\pi + \arccos(\frac{l_1^2+l_2^2-(x_b+l_0)^2-y_b^2}{2l_1l_2}),
\end{cases}
\label{eq:kinematyka_odwrotna}
\end{equation}

\section{Jakobian manipulatora}\label{sec:jakobian}
W manipulatorach równoległych, podobnie jak miało to miejsce w przypadku szeregowych, istnieje liniowa zależność pomiędzy prędkościami
napędzanych przegubów $\dot{q}$, a prędkościami liniowymi i kątowymi platformy (efektora), oznaczanymi jako $\dot{p}$ \cite{handbook}.
Zależność ta nazywana jest jakobianem manipulatora i definiuje się ją jako macierz \emph{J}:
\begin{equation}
\dot{p}=J(q)\dot{q},
\label{eq:jakobian}
\end{equation}
gdzie
\begin{equation}
J(p) = \frac{\delta p}{\delta q} = 
\begin{bmatrix}
\frac{\delta x_e}{\delta q_{a1}} & \frac{\delta x_e}{\delta q_{a2}} & \frac{\delta x_e}{\delta q_{b1}} & \frac{\delta x_e}{\delta q_{b2}} \\
\frac{\delta y_e}{\delta q_{a1}} & \frac{\delta y_e}{\delta q_{a2}} & \frac{\delta y_e}{\delta q_{b1}} & \frac{\delta y_e}{\delta q_{b2}} \\
\frac{\delta \theta_e}{\delta q_{a1}} & \frac{\delta \theta_e}{\delta q_{a2}} & \frac{\delta \theta_e}{\delta q_{b1}} & \frac{\delta \theta_e}{\delta q_{b2}}
\end{bmatrix}
\label{eq:jakobian2}
\end{equation}

Jakobian wykorzystywany jest do analizy błędów (dokładność pozycjonowania efektora jest zależna od precyzji sterowania napędów),
kalibracji, analizy statycznej, a także wyznaczania konfiguracji osobliwych. W manipulatorach 
równoległych proces obliczeniowy jakobianu
jest z reguły dużo bardziej skomplikowany niż dla manipulatorów szeregowych, głównie ze względu na uwikłaną postać równań kinematyki.
W najprostszej formie jest on wyznaczany
jako pochodne cząstkowe równań kinematyki (macierz Jacobiego), gdzie zmiennymi są napędy robota ($q_{a1}, q_{a2}, q_{b1}, q_{b2}$). 

Również w przypadku rozpatrywanego manipulatora macierz ta jest bardzo skomplikowana (obliczenia zostały wykonane z wykorzystaniem
środowiska \emph{Mathematica}). Ze względu na ich złożoność, wyprowadzenie równań jakobianu zamieszczono w dodatku. 


Korzystając z równania \eqref{eq:jakobian} możliwe jest obliczenie prędkości obrotowych, które pojawiają się w przegubach
przy zadanej prędkości efektora w przestrzeni zadaniowej. Własność ta jest bardzo przydatna przy doborze napędów robota,
ponieważ dla założonej maksymalnej prędkości efektora wyznaczane są maksymalne prędkości w przegubach, będące
jednymi z istotniejszych parametrów silników. Biorąc pod uwagę fakt, że przekładnie zwiększają momenty silników kosztem prędkości obrotowej
(wyrażanej w obrotach na minutę), konieczne jest wypracowanie kompromisu zapewniającego najefektywniejszą pracę manipulatora.

Zadanie wyznaczania maksymalnych prędkości obrotowych przegubów robota można zdefiniować jako problem optymalizacyjny, w którym to
przy zadanych ograniczeniach na ruch manipulatora (kąty przegubów), a także zadanej przestrzeni dopuszczalnych prędkości efektora
należy znaleźć maksymalną dopuszczalną wartość prędkości obrotowej jednego z przegubów. Zadanie to można formalnie przedstawić jako
problem optymalizacji z ograniczeniami:
\begin{itemize}
\item Ograniczenia:
\begin{itemize}
\item $q \in Q$,
\item $\dot{p} \in P$,
\item $\dot{p}=J(q)\dot{q}$,
\end{itemize}
\item Funkcje kryterialne:
\begin{itemize}
\item $\dot{q}^2_1$,
\item $\dot{q}^2_2$.
\end{itemize}
\end{itemize}
Przestrzenie Q oraz P składają się z założonych dopuszczalnych wartości położeń napędów i prędkości efektora. Kwadraty prędkości
w funkcjach kryterialnych wynikają z potrzeby jednakowej analizy wartości dodatnich i ujemnych. Dla rozpatrywanego manipulatora
wystarczy wziąć pod uwagę jedynie napędy $q_{a1}$ i $q_{a2}$, ponieważ ze względu na symetryczność pozostałe dwa napędy
powinny wykazywać te same właściwości.

Proces obliczeniowy umożliwiający rozwiązanie powyższego zadania optymalizacyjnego został przeprowadzony z wykorzystaniem
środowiska \emph{Mathematica}, które zawiera procedurę \texttt{NMaximize} służącą do znajdywania maksimum funkcji
kryterialnej przy zadanych ograniczeniach na podstawie obliczeń numerycznych. Sposób wywołania procedury znajduje się
na listingu \ref{listing}?, natomiast wyniki obliczeń zawiera tabela \ref{tab:maksymalne_predkosci}.


\begin{table}[tp]
  \caption{Analiza statyczna}
  \label{tab:analiza_statyczna}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Parametr} & {\bf Wartość} \\\hline\hline
$\dot{q}^2_{a1}$ & 2222222 [$\frac{rad}{s}$]\\\hline
$\dot{q}_{a2}$ & 2222222 [$\frac{rad}{s}$]\\
$\dot{q}_{b1}$ & 2222222 [$\frac{rad}{s}$]\\
$\dot{q}_{b2}$ & 2222222 [$\frac{rad}{s}$]\\
$q_{a1}$ & 2222222 [rad]\\
$q_{a2}$ & 2222222 [rad]\\
$q_{b1}$ & 2222222 [rad]\\
$q_{b2}$ & 2222222 [rad]\\\hline\hline
$\dot{q}^2_{a2}$ & 2222222 [$\frac{rad}{s}$]\\\hline
$\dot{q}_{a1}$ & 2222222 [$\frac{rad}{s}$]\\
$\dot{q}_{b1}$ & 2222222 [$\frac{rad}{s}$]\\
$\dot{q}_{b2}$ & 2222222 [$\frac{rad}{s}$]\\
$q_{a1}$ & 2222222 [rad]\\
$q_{a2}$ & 2222222 [rad]\\
$q_{b1}$ & 2222222 [rad]\\
$q_{b2}$ & 2222222 [rad]\\
\hline\hline
  \end{tabular}
\end{table}


\section{Analiza statyczna}\label{sec:analiza_statyczna}
Jednym z zastosowań jakobianu jest analiza statyczna. Umożliwia ona wyznaczenie wektora momentów pojawiających się w napędach
na skutek sił zewnętrznych działających na manipulator (równanie \ref{eq:analiza_statyczna}). 
Korzystając jakobianu opisanego równaniem \eqref{eq:jakobian2}, który jest liczony
na podstawie przekształcenia (kinematyki) od napędów do efektora, możliwe jest wyznaczenie momentów w silnikach generowanych
przez siły działające na efektor. Momenty i siły w innych punktach mogą być wyznaczone po obliczeniu równań kinematyki dla tych punktów.
\begin{equation}
\tau=J^Tf.
\label{eq:analiza_statyczna}
\end{equation}

W równaniu \ref{eq:analiza_statyczna} \emph{$\tau$} oznacza wektor momentów napędów (według kolejności ze wzoru \ref{eq:jakobian2}), natomiast
\emph{f} definiuje wektor sił działających na efektor (jego rozmiar zależy od liczby stopni swobody manipulatora) w bazowym
układzie współrzędnych manipulatora. Dla projektowanego manipulatora są to siły X, Y oraz moment $\theta$.

Analiza statyczna ma istotny wpływ na dobór silników do manipulatora. Dla założonej maksymalnej siły działającej na efektor
określone zostały wymagane momenty maksymalne pojawiające się na silnikach. Jest to jedno z kryteriów doboru silników, 
które zostało wzięte pod uwagę w rozdziale \ref{sec:dobor_silnikow}. W tabeli \ref{tab:analiza_statyczna} znajdują się
maksymalne momenty mogące się pojawić na silnikach dla kilku różnych wartości sił zewnętrznych działających na efektor
przy stałej, przykładowej konfiguracji manipulatora:
\begin{itemize}
\item $l_0=0.1m$,
\item $l_1=0.22m$,
\item $l_2=0.33m$,
\item $l_3=0.35m$,
\item $l_4=0.1m$,
\item $q_{a1}=\frac{\pi}{3}$,
\item $q_{a2}=\frac{\pi}{2}$,
\item $q_{b1}=\frac{2\pi}{3}$,
\item $q_{b2}=-\frac{\pi}{2}$,
\end{itemize}

Obliczenia zostały wykonane z wykorzystaniem środowiska \emph{Mathematica}, na podstawie wzoru \ref{eq:analiza_statyczna}, a także
jakobianu opisanego w rozdziale \ref{sec:jakobian}.

\begin{table}[tp]
  \caption{Analiza statyczna}
  \label{tab:analiza_statyczna}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Wektor sił/momentów [N]/[Nm]} & {\bf Momenty w napędach [Nm]} \\
{\bf ($f_x, f_y, m_{\theta}$)} & {\bf ($\tau_{a1}, \tau_{a2}, \tau_{b1}, \tau_{b2}$)} \\\hline\hline
(-10, 0, 0) & (3.34, 7.50, 10.46, 10.8) \\
(-100, 0, 0) & (33.45, 75.03, 104.55, 108.03) \\
(0, -10, 0) & (-0.48, -1.25, -3.03, -4.46) \\
(0, -100, 0) & (-4.85, -12.46, -30.31, -44.7) \\
(0, 0, 10) & (11.60, 18.85, 11.60, 18.85) \\
(0, 0, 100) & (115.97, 188.54, 115.93, 188.54) \\
(-100, -100, 0) & (28.60, 62.57, 74.24, 63.33) \\
(100, 100, 0) & (-28.60, -62.57, -74.24, -63.33) \\
(100, 100, 100) & (87.37, 125.97, 41.73, 125.21) \\
\hline\hline
  \end{tabular}
\end{table}
\section{Wyważenie statyczne}
Kolejnym ciekawym problemem statycznym dotyczącym manipulatorów równoległych jest wyważenie statyczne \cite{handbook}.
Manipulator równoległy jest wyważony statycznie, gdy waga poszczególnych ogniw nie powoduje w bezruchu jakichkolwiek 
momentów w napędach dla każdej konfiguracji robota. Zapewnienie wyważenia statycznego bardzo często dokonywane jest za pomocą
sprężyn, co prowadzi do konieczności utrzymania stałej energii potencjalnej układu, na którą składają się energia elastyczności sprężyn, 
a także grawitacja. W przypadku braku sprężyn należy zadbać, by położenie środka masy robota nie zmieniało się wzdłuż wektora
grawitacji (jedynie prostopadle do niego).

Planarna, prostopadła do siły grawitacji budowa projektowanego manipulatora implikuje stałą energię potencjalną układu. W związku z tym,
bez potrzeby wprowadzania sprężyn konstruowany robot jest wyważony statycznie. Należy jednakże pamiętać, że niewielkie
zmiany ustawienia manipulatora mogą prowadzić do zaburzeń energii potencjalnej układu.

\section{Dynamika manipulatora}
Sterowanie robotem najczęściej odbywa się z wykorzystaniem modelu dynamiki. Ciała sztywne stanowiące ogniwa
manipulatora wzbogacone zostają (w stosunku do modelu kinematyki) o rozmiary geometryczne, masę i bezwładność \cite{podstawy_robotyki}.
Sterowanie oparte na modelu dynamiki lepiej sprawdza się przy precyzyjnym śledzeniu zadanej trajektorii, w szczególności
w przypadku rygorystycznych ograniczeń nakładanych na prędkości i przyspieszenia w ruchu robota. 

Ze względu na przyjętą skomplikowaną strukturę manipulatora model dynamiki w niniejszym projekcie zostanie pominięty. Zadania stawiane
przed robotem nie wymagają precyzyjnego zadawania prędkości i przyspieszeń, a zamknięty, równoległy łańcuch kinematyczny
zapewnia odpowiednią sztywność. Ponadto, redundancja i podatność robota zapewnia odpowiednią reakcję na błędy i niedokładności
przy pozycjonowaniu ogniw i efektora.


\chapter{Wyznaczenie wymiarów manipulatora}\label{sec:wyznaczenie_wymiarow_manipulatora}
Po zdefiniowaniu modelu robota w postaci równań kinematyki można przejść do projektowania
jego fizycznej konstrukcji. Pierwszym etapem tego procesu jest określenie żądanych gabarytów
robota. Istnieje kilka podejść do tego zadania \ref{sec:aspekt_badawczy}, jednakże w przypadku tej pracy została
wykorzystana analiza stosunku wielkości przestrzeni roboczej do rozmiarów poszczególnych
elementów manipulatora.

\section{Obliczenie przestrzeni roboczej manipulatora}\label{sec:obliczenie_przestrzeni_roboczej}
W celu obliczenia przestrzeni roboczej manipulatora stworzony został oddzielny program w języku C++, 
który realizował to zadanie. Składał się on przede wszystkim z klasy symulującej obiekt manipulatora, 
w której zaimplementowane zostały metody liczenia zarówno prostej jak i odwrotnej kinematyki dla
konkretnej instancji robota. Na ich podstawie wyznaczana jest przestrzeń robocza. Wyniki obliczeń
z wykorzystaniem jednej i drugiej metody zapisywane są do tej samej postaci, co pozwala na ich porównanie.

Postać ta zakłada stworzenie odpowiednio dużej siatki w przestrzeni (większej niż
przestrzeń robocza manipulatora) o określonych i równych rozmiarach pojedynczych komórek
wypełnionych zerami. Następnie wypełniamy wartościami jeden wszystkie te komórki, które są dla
poprzez efektor osiągalne dla badanego manipulatora. 

\subsection{Przestrzeń robocza na bazie prostej kinematyki}
Prosta kinematyka manipulatora zaimplementowana analogicznie do obliczeń z rozdziału \ref{prosta_kinematyka},
tutaj jest już liczona dla konkretnych wartości parametrów manipulatora. 
W celu wyznaczenia przestrzeni roboczej rozpatrzone zostały wszystkie możliwe konfiguracje
kątów przegubów manipulatora, z dokładnością do zadanego kroku i z wyłączeniem konfiguracji
niedozwolonych (opisanych szerzej w rozdziale \ref{sec:konfiguracje_niedozwolone}).
Rezultatem wyznaczenia prostej kinematyki jest położenie XY, dla którego odpowiadająca komórka
siatki przestrzeni (ta, w której efektor w zadanej konfiguracji się znajduje)
zostaje wypełniona jedynką. 

Zwiększenia dokładności obliczeń spowodowało znaczące wydłużenie czasu działania programu. Jako rozwiązanie
zastosowano iterację kolejnych położeń przegubów z pięciokrotnie większym krokiem, a dokładną analizę przeprowadzano jedynie
dla tych, które znajdywały się w otoczeniu (w sensie przestrzeni Q) konfiguracji poprawnych.

\subsection{Przestrzeń robocza na bazie odwrotnej kinematyki}
W przypadku odwrotnej kinematyki stosujemy odwrotne podejście do problemu wyznaczania przestrzeni roboczej. 
Tym razem zadanie sprowadza się do przejrzenia wszystkich komórek siatki przestrzeni i oznaczeniu
jedynką tych, dla których możliwe jest wyznaczenie konfiguracji manipulatora, w której efektor
znajduje się w aktualnej komórce. W związku z tym funkcja wyznaczająca odwrotną kinematyką
dla zadanego położenia zwraca wartość \emph{true/false} w zależności od tego czy operacja
się powiodła. 

Podobnie jak w przypadku kinematyki prostej zwiększenie dokładności obliczeń negatywnie wpłynęło na czas obliczeń. W związku z tym konieczne okazało się
zmodyfikowanie algorytmu w celu redukcji złożoności obliczeniowej. Wykorzystane zostało założenie mówiące
o spójności przestrzeni roboczej manipulatora -- jest ona jednym, zwartym obszarem. W konsekwencji, możliwe jest
wystartowanie obliczeń ze środka tego obszaru i w momencie napotkania końca przestrzeni roboczej zakończenie obliczeń.
Pozwoliło to na znaczące zwiększenie dokładności obliczeń przy stałym czasie działania programu.

\subsection{Obwiednia przestrzeni roboczej}
Kolejnym etapem liczenia powierzchni jest wyznaczenie obwiedni przestrzeni roboczej na podstawie
siatki wypełnionej z wykorzystaniem metod kinematyki. W tym celu został zaimplementowany algorytm, 
który dla zadanej siatki tworzy jej kopię zawierającą jedynie obrys przestrzeni. Co warto dodać,
dla efektywności obliczeń nie przeszukuje on całej siatki, a jedynie inteligentnie porusza się
po krawędziach przestrzeni roboczej (zakładamy, że jest ona wypukła). 

Dodatkowo w algorytmie została zaimplementowana możliwość zapisania wygenerowanego obrysu do pliku. 
Odbywa się to poprzez przeliczenie odpowiednich komórek siatki na wartości X i Y co umożliwia
późniejsze narysowanie przestrzeni. Przykład takiej obwiedni, wygenerowany z pomocą programu gnuplot
został przedstawiony na rysunku \ref{rys:przestrzen_robocza}. 

\subsection{Wyznaczanie powierzchni przestrzeni roboczej} 
Na podstawie obwiedni przestrzeni roboczej jesteśmy w stanie obliczyć jej powierzchnię. 
Ze względu na ograniczenia numeryczne przyjęte wcześniej będzie to jedynie jej aproksymacja. 
Dla każdej kolumny obliczamy liczbę komórek siatki pomiędzy wystąpieniem pierwszej i drugiej jedynki
(górna i dolna krawędź obrysu), a następnie sumujemy wszystkie te wartości otrzymując powierzchnię przestrzeni
roboczej. Dokładność otrzymanej w ten sposób powierzchni zależy w dużym stopniu od zdefiniowanej
ziarnistości siatki. 

\section{Wyznaczenie wymiarów manipulatora na podstawie przestrzeni roboczej}
Wzajemna relacja długości ogniw manipulatora ma ogromny wpływ na jego możliwości manipulacyjne. W związku z tym definiuje się
zadanie optymalizacyjne, mające na celu wyznaczenie takiej konfiguracji długości ramion robota, która zapewnia największą
wartość kryterium jakości. Za funkcję kryterialną, która podlega zadaniu maksymalizacji, przyjęta została powierzchnia 
przestrzeni roboczej manipulatora.
Posiadając możliwość obliczenia jej rozmiaru dla pojedynczej instancji 
manipulatora jesteśmy w stanie porównać je i wybrać tą, która zapewni nam największą wartość funkcji kryterialnej.
W tym celu definiujemy konkretną wartość jako sumę ramion (sama wartość nie jest istotna, gdyż
interesuje nas wzajemny stosunek długości ramion). Następnie zmieniamy długość każdego z ogniw z odpowiednim krokiem
i obliczamy rozmiar przestrzeni roboczej, zarówno prostą jak i odwrotną kinematyką. Oczywiście interesują nas tylko te
konfiguracje, w których suma długości ramion nie przekracza zadanej sumy. Spośród wszystkich wygenerowanych kombinacji wybieramy tą,
która maksymalizuje rozmiar przestrzeni roboczej. Dla zdefiniowanych ogniw należy wyznaczyć także optymalne rozstawienie
początków każdego z ramion (silników). Operację tą wykonujemy dla konkretnych długości ogniw, które z kolei musimy liczyć dla 
konkretnego rozstawienia -- zadania te są komplementarne.

\subsection{Wyznaczenie wymiarów próbnej wersji manipulatora}\label{sec:wymiary_probnej_wersji}
Przed przystąpieniem do wyznaczania konfiguracji docelowego manipulatora proces optymalizacji został przeprowadzony dla wersji próbnej,
opisanej w rozdziale \ref{rys:probna_wersja}. W tym przypadku dokładność wyniku nie była najistotniejsza, w związku z czym
wszystkie długości iterowano z krokiem 10, przy czym ich suma powinna wynosić 100. 
Po zaimplementowaniu prostej i odwrotnej kinematyki na początku wyznaczono optymalne
konfiguracje dla kilku przykładowych wartości L, będących połową odległości pomiędzy początkami ramion manipulatora:
\begin{itemize}
\item kinematyka prosta:
\begin{itemize}
\item $l_0=0:$ $l_1=40$, $l_2=20$, $l_3=40$, $l_4=0$,
\item $l_0=10:$ $l_1=30$, $l_2=30$, $l_3=40$, $l_4=0$,
\item $l_0=20:$ $l_1=20$, $l_2=40$, $l_3=40$, $l_4=0$,
\item $l_0=30:$ $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$,
\item $l_0=40:$ $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$,
\end{itemize}
\item kinematyka odwrotna:
\begin{itemize}
\item $l_0=0:$ $l_1=30$, $l_2=40$, $l_3=30$, $l_4=0$,
\item $l_0=10:$ $l_1=30$, $l_2=40$, $l_3=30$, $l_4=0$,
\item $l_0=20:$ $l_1=30$, $l_2=30$, $l_3=40$, $l_4=0$,
\item $l_0=30:$ $l_1=30$, $l_2=20$, $l_3=50$, $l_4=0$,
\item $l_0=40:$ $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$,
\end{itemize}
\end{itemize}
Warto wspomnieć, że parametry $l_1-l_4$ były iterowane na przedziale od 0 do 50. Jak widzimy, dla tej wersji manipulatora ostatnie z ramion
najmniej wpływa na wielkość przestrzeni roboczej, w związku z czym algorytm starał się je eliminować (dzięki temu inne ramiona mogły by dłuższe).
Przy oddalaniu początków ramion od siebie wzrasta znaczenie trzeciego ramienia, podczas gdy maleje pierwszego. Różnice pomiędzy prostą
i odwrotną kinematyką wynikają głównie z różnych metodologii liczenia, jednakże warto rozważyć i jedną i drugą opcję w celu zebrania
większej ilości obserwacji. Posiadając kilka wybranych konfiguracji manipulatora możemy teraz dokładniej już (z krokiem 1) znaleźć
najlepszą dla nich odległość $l_0$. Wyniki zostały zaprezentowane na wykresach, rysunek \ref{rys:probna_wersja_prosta_kin} prosta kinematyka
i rysunek \ref{rys:probna_wersja_odwrotna_kin}. Jak można się było spodziewać, w obu przypadkach największa przestrzeń robocza jest
osiągana dla małych wartości $l_0$. Jednakże jest to sprzeczne z wymaganiem dotyczącym rozłożenia sił działających na efektor na
poszczególne napędy -- zależy nam, aby ramiona były w pewnej odległości od siebie. W związku z tym konieczne jest wypracowanie konsensusu.
Wszystkie konfiguracje długości zwracają stosunkowo dość duże przestrzenie robocze dla parametru $l_0$ znajdującego się w przedziale (10,20).
Jeżeli to byłaby ostateczna wersja manipulatora, jako kompromis wartość z tego przedziału zostałaby wybrana.


\begin{figure}[tp]
  \setlength{\unitlength}{1.0cm}
  \centering
	\subfigure[$l_0=0$: $l_1=40$, $l_2=20$, $l_3=40$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Forward/L=0}
	}
	\subfigure[$l_0=10$: $l_1=30$, $l_2=30$, $l_3=40$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Forward/L=10}
	} \\
	\subfigure[$l_0=20$: $l_1=20$, $l_2=40$, $l_3=40$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Forward/L=20}
	}
	\subfigure[$l_0=30$: $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Forward/L=30}
	} \\
	\subfigure[$l_0=40$: $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Forward/L=40}
	}
  \label{rys:probna_wersja_prosta_kin}  
  \caption{Powierzchnia przestrzeni roboczej w zależności od odległości początków pierwszych ramion 
  manipulatora przy wykorzystaniu prostej kinematyki}

\end{figure}

\begin{figure}[tp]
  \setlength{\unitlength}{1.0cm}
  \centering
	\subfigure[$l_0=0$: $l_1=30$, $l_2=40$, $l_3=30$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Inverse/L=0_inv}
	}
	\subfigure[$l_0=10$: $l_1=30$, $l_2=40$, $l_3=30$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Inverse/L=10_inv}
	} \\
	\subfigure[$l_0=20$: $l_1=30$, $l_2=30$, $l_3=40$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Inverse/L=20_inv}
	}
	\subfigure[$l_0=30$: $l_1=30$, $l_2=20$, $l_3=50$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Inverse/L=30_inv}
	} \\
	\subfigure[$l_0=40$: $l_1=20$, $l_2=30$, $l_3=50$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/probna/Inverse/L=40_inv}
	}
  \label{rys:probna_wersja_odwrotna_kin}  
  \caption{Powierzchnia przestrzeni roboczej w zależności od odległości początków pierwszych ramion 
  manipulatora przy wykorzystaniu odwrotnej kinematyki}

\end{figure}

\subsection{Wyznaczenie wymiarów ostatecznej wersji manipulatora}
Analogiczne badania zostały przeprowadzone dla manipulatora docelowego, opisanego w rozdziale \ref{sec:struktura_manipulatora_docelowego}.
Przestrzeń robocza tego robota jest symetryczna, a także bardziej regularna niż w przypadku wersji próbnej -- brzegi przestrzeni są
krzywymi gładkimi, przez co jej kształt jest zbliżony do postaci wypukłej. 
Dla wersji docelowej zwiększona została dokładność obliczeń, co zaowocowało wydłużonym czasem działania
programu. Większą dokładność wprowadzono poprzez zwiększenie ziarnistości siatki przestrzeni roboczej, a także poprzez zmniejszenie kroku algorytmu,
który odpowiadał za wyznaczanie kolejnych konfiguracji. Ponadto długość każdego z ramion iterowana była o wartość 1 (a nie 5 jak poprzednio), 
w związku z czym optymalny stosunek długości ramion został wyznaczony bardziej precyzyjnie. Ogniwa $l_1-l_4$ iterowane były na przedziale
10-45, a ich suma powinna równać się 100. Jak się okazało, ogniwo $l_4$ ponownie ma najmniejszy wpływ na rozmiar przestrzeni roboczej,
w związku z czym algorytm zwracał zawsze wartość $l_4=10$.

Poniżej znajdują się optymalne konfiguracje manipulatora dla kilku wybranych parametrów $l_0$, a także kinematyk prostej i odwrotnej
(wyniki różnią się ze względu na metodologię liczenia przestrzeni, jednakże tendencje są zachowane):
\begin{itemize}
\item kinematyka prosta:
\begin{itemize}
\item $l_0=0:$ $l_1=31$, $l_2=28$, $l_3=31$, $l_4=10$,
\item $l_0=5:$ $l_1=24$, $l_2=34$, $l_3=32$, $l_4=10$,
\item $l_0=10:$ $l_1=22$, $l_2=33$, $l_3=35$, $l_4=10$,
\item $l_0=15:$ $l_1=24$, $l_2=25$, $l_3=41$, $l_4=10$,
\item $l_0=20:$ $l_1=25$, $l_2=20$, $l_3=45$, $l_4=10$,
\item $l_0=25:$ $l_1=18$, $l_2=28$, $l_3=44$, $l_4=10$,
\item $l_0=30:$ $l_1=16$, $l_2=29$, $l_3=45$, $l_4=10$,
\end{itemize}
\item kinematyka odwrotna:
\begin{itemize}
\item $l_0=0:$ $l_1=43$, $l_2=28$, $l_3=19$, $l_4=10$,
\item $l_0=5:$ $l_1=40$, $l_2=24$, $l_3=26$, $l_4=10$,
\item $l_0=10:$ $l_1=39$, $l_2=25$, $l_3=26$, $l_4=10$,
\item $l_0=15:$ $l_1=35$, $l_2=23$, $l_3=32$, $l_4=10$,
\item $l_0=20:$ $l_1=33$, $l_2=18$, $l_3=39$, $l_4=10$,
\item $l_0=25:$ $l_1=25$, $l_2=24$, $l_3=41$, $l_4=10$,
\item $l_0=30:$ $l_1=28$, $l_2=18$, $l_3=44$, $l_4=10$,
\end{itemize}
\end{itemize}

Tak jak poprzednio przy małych odległościach $l_0$ funkcja kryterialna wybierała ogniwo $l_1$ jako najważniejsze, przypisując mu
największe wartości. W miarę zwiększania $l_0$ maleje znaczenie pierwszego ogniwa, a rośnie trzeciego $l_3$. Takie same wnioski
można wysnuć zarówno z prostej jak i odwrotnej kinematyki, a rozbieżności w długościach ogniw ponownie wynikają z technik liczenia przestrzeni
roboczej -- są one zupełnie inne (rozdz. \ref{sec:obliczenie_przestrzeni_roboczej}).

Dla kilku wybranych konfiguracji możliwie najbardziej różniących się od siebie dla prostej i odwrotnej kinematyki
wykreślone zostały zależności powierzchni przestrzeni roboczej od odległości początków ramion $l_0$. Znajdują się one
na rysunkach \ref{rys:l0_prosta_kin} i \ref{rys:l0_odwrotna_kin}.

Największe rozmiary przestrzeni roboczej manipulatora otrzymywane są przy małych wartościach $l_0$, co niesie za sobą stosunkowo
długie ogniwo $l_1$. Takie konfiguracje niosą za sobą jednakże gorsze własności statyczne manipulatora, które
odpowiadają za rozkład sił. W konsekwencji, celem niniejszych obliczeń jest znalezienie takiej konfiguracji długości elementów $l_0 -- l_5$, 
która zapewni stosunkowo dużą przestrzeń roboczą przy dużych wartościach odległości $l_0$. 

Kierując się tym założeniem za najlepszy wybór należy uznać konfiguracje z rysunków \ref{rys:l0_prosta_kin_najlepszy} 
i \ref{rys:l0_odwrotna_kin_najlepszy}. Optymalne konfiguracje zarówno dla prostej jak i odwrotnej kinematyki okazały się w tych przypadkach zbliżone.
Zauważalny jest także w obu przypadkach wzrost powierzchni przestrzeni roboczej dla małych wartości $l_0$. W szczególności na rysunku 
\ref{rys:l0_prosta_kin_najlepszy} można zauważyć, że na pewnym zakresie parametru $l_0$ powierzchnia przestrzeni roboczej zmienia się w 
niewielkim stopniu, co może mieć pozytywny wpływ na możliwości ruchowe robota. 

Jako wynik przeprowadzonych eksperymentów można w przybliżeniu przyjąć następującą konfigurację: 
\begin{itemize}
\item $l_0=10$,
\item $l_1=28$,
\item $l_2=23$,
\item $l_3=41$,
\item $l_4=10$,
\end{itemize}
Długość ogniwa $l_4$ w każdym z algorytmów była minimalizowana, w związku z czym jej wybór może być dowolny.

Przy dobieraniu rzeczywistych długości ramion pod uwagę należy wziąć również mechaniczną konstrukcję robota, która
wprowadza pewne ograniczenia na długości ogniw. Ponadto, ramiona budowane są z dostępnych elementów, co dodatkowo ogranicza swobodę
doboru ich gabarytów. Przy analizie przestrzeni roboczej opierano się jedynie na kinematyce robota, która nie bierze
pod uwagę ani łączeń pomiędzy ogniwami, ani mechanicznych możliwości zastosowanych części. W związku z tym niniejsza analiza stanowi
jedynie podstawę wyboru elementów, a faktyczne długości ramion w oparciu o niniejsze rozważania zostały wyselekcjonowane w rozdziale
\ref{sec:mechaniczna_konstrukcja_manipulatora}.

\begin{figure}[tp]
  \setlength{\unitlength}{1.0cm}
  \centering
	\subfigure[$l_0=0:$ $l_1=31$, $l_2=28$, $l_3=19$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/For_L=0}
	}
	\subfigure[$l_0=10:$ $l_1=22$, $l_2=33$, $l_3=35$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/For_L=10}
	} \\
	\subfigure[$l_0=20:$ $l_1=25$, $l_2=20$, $l_3=45$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/For_L=20}
	  \label{rys:l0_prosta_kin_najlepszy}
	}
	\subfigure[$l_0=30:$ $l_1=16$, $l_2=29$, $l_3=45$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/For_L=30}
	}
  \label{rys:l0_prosta_kin}  
  \caption{Powierzchnia przestrzeni roboczej w zależności od odległości początków pierwszych ramion 
  manipulatora przy wykorzystaniu prostej kinematyki}

\end{figure}

\begin{figure}[tp]
  \setlength{\unitlength}{1.0cm}
  \centering
	\subfigure[$l_0=0$: $l_1=43$, $l_2=28$, $l_3=19$, $l_4=0$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/inv_L=0}
	}
	\subfigure[$l_0=10:$ $l_1=39$, $l_2=25$, $l_3=26$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/inv_L=10}
	} \\
	\subfigure[$l_0=20:$ $l_1=33$, $l_2=18$, $l_3=39$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/inv_L=20}
	}
	\subfigure[$l_0=30:$ $l_1=28$, $l_2=18$, $l_3=44$, $l_4=10$]{
	  \includegraphics[width=0.4\textwidth]{grafika/docelowa/inv_L=30}
	  \label{rys:l0_odwrotna_kin_najlepszy}
	}
  \label{rys:l0_odwrotna_kin}  
  \caption{Powierzchnia przestrzeni roboczej w zależności od odległości początków pierwszych ramion 
  manipulatora przy wykorzystaniu odwrotnej kinematyki}

\end{figure}
\chapter{Pomiar siły}
Podatność manipulatora zakłada precyzyjny pomiar sił, którymi oddziałuje on na swoje otoczenie. Rozpoznawanie
zarówno wartości, jak i kierunków sił jest zadaniem złożonym i wymaga zastosowania dużej liczby czujników, które
będą w stanie współpracować ze sobą i wzajemnie się uzupełniać. Najbardziej interesujące z punktu widzenia możliwych zastosowań
są wektory sił działających na efektor manipulatora. Z drugiej strony umieszczanie czujników na efektorze robota nie jest dobrym pomysłem,
ponieważ jest on najbardziej narażony na kontakt fizyczny, co mogłoby prowadzić do uszkodzeń mechanicznych czujników. Ponadto
prowadzenie przewodów w pobliżu efektora mogłoby niekorzystnie wpłynąć na jego zdolności manipulacyjne, a także ze względu na odległość zwiększyć
szum pomiaru. W związku z tym do analizy sił zewnętrznych zdecydowano się zastosować tensometry \ref{sec:tensometry} umieszczone na 
ramionach manipulatora, 
a także pomiar poboru prądu silników \ref{sec:pomiar_pradu_silnikow}, który jest zależny od pojawiających się momentów. Wyznaczenie wpływu sił działających
na efektor na momenty napędowe pojawiające się w silnikach wymaga dodatkowych obliczeń, które zostały przeprowadzone w sekcji \ref{sec:rozklad_sil}.

\section{Rozkład sił}\label{sec:rozklad_sil}

\section{Pomiar prądu silników}\label{sec:pomiar_pradu_silnikow}

\section{Tensometry}\label{sec:tensometry}
Do pomiaru sił oddziaływania efektora na otoczenie można wykorzystać także tensometry \cite{tensometry}.
Istnieje wiele rodzajów tensometrów, które różnią się między sobą rozmiarem, dokładnością i odpornością na uszkodzenia mechaniczne.
Ze względu na dużą dokładność, a także przystępną cenę wybrane zostały tensometry foliowe (rys. \ref{rys:tensometr}), 
które należą do rodziny tensometrów oporowych. W ostatecznej wersji manipulatora zdecydowano się na gotowe układy
tensometryczne zwane belkami tensometrycznymi, opisane w rozdziale \ref{sec:belki_tensometryczne}. 
Symbol rozpatrywanego wcześniej czujnika to \emph{TEN-TF5/120-P}, a jego parametry znajdują się w tabeli \ref{tab:tensometr}.
W tensometrii elektrooporowej wykorzystuje się zjawisko zmiany rezystancji przewodnika wynikającej z jego wydłużenia lub skrócenia.
\begin{table}[tp]
  \caption{Parametry tensometru}
  \label{tab:tensometr}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Parametr} & {\bf Wartość} \\
Szerokość & 4.5 mm \\
Długość & 11 mm \\
Grubość & 60 $\mu$m \\
Rezystancja & $120^{\pm0.2\%}\Omega$ \\
Maks. natężenie prądu & 50 mA \\
Wytrzymałość zmęczeniowa & $n>10^7$ dla $\varepsilon = 1\%$ \\
Stała tensometru & 2.15 \\
Temperatura pracy & -40 .. $200^o C$ \\
Odkształcenie maks. & 5\% \\
Tolerancja & $\pm 0.5$\% \\ 
\hline\hline
  \end{tabular}
\end{table}

\begin{figure}[tp]
\centering
  \includegraphics[width=0.4\textwidth]{grafika/tensometr}
  \caption{Tensometry foliowe}
  \label{rys:tensometr}  
\end{figure}

Mimo iż tensometry tego typu głównie wykorzystywane są do mierzenia sił działających na wyróżnione pole od góry, zmiana rezystancji 
następuje także przy ściskaniu i rozciąganiu całego czujnika. Umożliwia to pomiar naprężeń działających na pojedyncze ogniwo
manipulatora. Lokując tensometry na manipulatorze w odpowiedni sposób, zyskujemy możliwość obliczenia wektorów (wartości i kierunków)
sił działających na efektor.

\subsection{Układ pomiarowy}
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/mostek}
  \caption{Dwie równoważne reprezentacje mostka Wheatstone'a}
  \label{rys:mostek}  
\end{figure}
Do pomiaru zmiany rezystancji tensometru najczęściej wykorzystuje się mostek Wheatstone'a \cite{tensometry}. Rysunek \ref{rys:mostek}
przedstawia dwa równoważne schematy, w których układ ten został zastosowany. Rezystory $R_1, R_2, R_3, R_4$ nazywane są ramionami mostka,
$V_s$ jest napięciem pobudzenia mostka, natomiast $V_0$ jest napięciem wyjściowym, które mierzymy.

Mostek Wheatstone'a używany jest nie tylko do pomiarów
absolutnej wartości rezystancji, ale także jej zmian, co jest istotne z punktu widzenia tensometrów. Największą zaletą
tego układu jest bardzo dużą dokładność, rzędu od $10^{-4}$ do $10^{-2}$ $\frac{\Omega}{\Omega}$.

Istnieje kilka konfiguracji mostka Wheatstone'a, w zależności od liczby zmieniających się rezystancji.
W najprostszym przypadku, jedynie $R_1$ ulega wariacjom, natomiast pozostałe rezystancje są stałe. Otrzymany w ten sposób
mostek jest najbardziej podatny na nieliniowości. Możliwe są też konfiguracje w których dwie, bądź nawet wszystkie cztery rezystancje
mogą ulegać zmianie -- jak się okazuje jest to często wykorzystywana praktyka. Podstawowym wymaganiem dla wszystkich konfiguracji
jest zapewnienie równości pomiędzy odpowiednimi rezystancjami w sytuacji, gdy układ znajduje się w stanie równowagi: $R_1 = R_2$ i
$R_3 = R_4$.
\begin{equation}
V_0 = V_s (\frac{R_1}{R_1+R_2} - \frac{R_4}{R_3+R_4}).
\label{eq:mostek_v0}
\end{equation}
Wzór \ref{eq:mostek_v0} pozwala obliczyć napięcie wyjściowe mostka. Jeżeli układ jest w równowadze (w przypadku tensometrów nie działają
żadne siły), to powinien zachodzić warunek $\frac{R_1}{R_2} = \frac{R_4}{R_3}$, który implikuje $V_0 = 0$.
W momencie, gdy pojawiają się siły do rezystancji tensometrów dodawana jest wartość $\Delta R$, która jest w przybliżeniu
wprost proporcjonalna do przyłożonej siły. W ten sposób wzór \ref{eq:mostek_v0} przekształca się do wzoru \ref{eq:mostek_v02}.
\begin{equation}
V_0 = V_s (\frac{R_1 + \Delta R_1}{R_1 + \Delta R_1 +R_2  + \Delta R_2} - \frac{R_4 + \Delta R_4}{R_3 + \Delta R_3 + R_4 + \Delta R_4}).
\label{eq:mostek_v02}
\end{equation}

Po kolejnych przekształceniach i nieznacznych uproszczeniach dochodzimy do wzoru \ref{eq:mostek_v03}, w którym to
mamy bezpośrednią zależność napięcia od zmian rezystancji.
\begin{equation}
\frac{V_0}{V_s} = \frac{1}{4}(\frac{\Delta R_1}{R_1} - \frac{\Delta R_2}{R_2} + \frac{\Delta R_3}{R_3} - \frac{\Delta R_4}{R_4}).
\label{eq:mostek_v03}
\end{equation}

W ostatnim kroku obliczeń należy jeszcze wziąć pod uwagę stałą tensometru k. W efekcie, otrzymujemy równanie \ref{eq:mostek_v04}.

\begin{equation}
\frac{V_0}{V_s} = \frac{k}{4}(\varepsilon_1 - \varepsilon_2 + \varepsilon_3 - \varepsilon_4),
\label{eq:mostek_v04}
\end{equation}
gdzie
\begin{equation}
\frac{\Delta R}{R} = k \cdot \varepsilon.
\label{eq:mostek_v05}
\end{equation}


\section{Belki tensometryczne}\label{sec:belki_tensometryczne}
Zastosowanie dwóch kompletnych belek tensometrycznych (rys. \ref{rys:belka_tensometryczna}), złożonych z trzech tensometrów każda, 
pozwoliło na zredukowanie złożoności aspektu elektronicznego projektu. Ponadto, gotowe układy z reguły cechują się lepszymi
parametrami, ponieważ zostały wcześniej wspólnie przetestowane (szczególne znaczenie ma tutaj wpływ temperatury na pomiary tensometrów).
Wykorzystane belki, opisywane symbolem \emph{NA27}, najczęściej używane są do precyzyjnych wag elektronicznych. Ich podstawowe
parametry zamieszczono w tabeli \ref{tab:belka_tensometryczna}. Każda z belek zawiera wbudowane mostki Wheatstone'a, które
zapewniają właściwą kompensację temperaturową. Na zewnątrz wyprowadzone są jedynie cztery przewody, opisane w tabeli \ref{tab:wyprowadzenia_belki}.
\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/belka_tensometryczna}
  \caption{Belka tensometryczna \emph{NA27}}
  \label{rys:belka_tensometryczna}  
\end{figure}

\begin{table}[tp]
  \caption{Parametry belki tensometrycznej}
  \label{tab:belka_tensometryczna}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
    {\bf Parametr} & {\bf Wartość} \\
Zakres ważenia & do 5 kg \\
Sygnał wyjściowy max. & $1.0\pm 0.15\frac{mV}{V}$ \\
Błąd pomiarowy & 0.03\% F.S \\
Rezystancja wyjściowa mostka & 1000 $\pm 100\Omega$ \\
Napięcie wzbudzenie mostka & 5 \textdiv 12V DC \\
Roboczy zakres temperatury pracy & $-20^oC$ \textdiv $+60^oC$ \\
Bezpieczne przeciążenie & 150\% wartości sygnału wyjściowego \\
Materiał & stop aluminium \\
\hline\hline
  \end{tabular}
\end{table}

\begin{table}[tp]
  \caption{Opis kabli belki tensometrycznej}
  \label{tab:wyprowadzenia_belki}
  \centering
  \begin{tabular}{|c|c|c|c|}
    \hline
Pobudzenie $U_{we+}$ & Pobudzenie $U_{we-}$ & Wyjście $U_{wy+}$ & Wyjście $U_{wy-}$\\
\hline
czerwony & czarny & zielony & biały \\
\hline
  \end{tabular}
\end{table}

Mimo iż belki tensometryczne służą do ważenia położonych na nich obiektów, możliwy jest także pomiar ich naprężeń (wynika to
z własności tensometrów opisanych w rozdziale \ref{sec:tensometry}). W projektowanym manipulatorze belki będą wchodziły w skład ogniw $l_3$, aby mierzyć
kierunki i wartości sił działających na efektor na oba ramiona robota. Jako że jest to manipulator planarny, mierzone będą jedynie siły działające w
jednej płaszczyźnie. Belki zostaną zamocowane tak, aby dwa tensometry mierzyły te siły, natomiast zastosowanie trzeciego tensometru ograniczy się do
kompensacji temperaturowej.

\subsection{Testy}
Przed przystąpieniem do eksploatacji urządzenia elektronicznego należy przetestować jego właściwości. Jednym z istotniejszych parametrów
jest liniowość charakterystyki, która pozwala ocenić przewidywalność i powtarzalność wartości otrzymywanych na wyjściu przy zmianach
wejścia (w tym przypadku naprężenia). W tym celu skonstruowany został podstawowy układ pomiarowy przedstawiony na schemacie \ref{rys:pomiar_belki}.

\begin{figure}[tp]
\centering
  \includegraphics[width=0.4\textwidth]{grafika/pomiar_belki}
  \caption{Podstawowy schemat badający liniowość belki tensometrycznej}
  \label{rys:pomiar_belki}  
\end{figure}

Znana z dokładnością do jednego grama masa (mierzona za pomocą elektronicznej wagi) wieszana była na haczyku przykręconym do belki
tensometrycznej. Z drugiej strony belka zamocowana była sztywno w imadle, prostopadle do powierzchni ziemi. W konsekwencji, działająca
na masę zawieszoną na haczyku siła grawitacji miała znaną wartość i mogła być zmierzona przez tensometry wchodzące w skład belki.
Pomiar tej siły odbywał się za pomocą woltomierza, który mierzył napięcie na wyjściu mostka Wheatstone'a z dokładnością do 0,1 mV.
Napięcie zasilania mostka Wheatstone'a, doprowadzone z mikrokontrolera STM32 (takie będzie wykorzystane w manipulatorze docelowym),
wynosiło +5V. Wykres zależności napięcia wyjściowego od przyłożonej do belki siły zawiera rysunek \ref{rys:belka_liniowosc}, natomiast
dane pomiarowe znajdują się w tabeli \ref{tab:belka_liniowosc}. Do obliczeń przyjęto przyspieszenie ziemskie wynoszące $g=9.81\frac{m}{s^2}$.

\begin{figure}[tp]
\centering
  \includegraphics[width=0.4\textwidth]{grafika/belka_liniowosc}
  \caption{Zależność napięcia wyjściowego od siły przyłożonej na jednym z końców belki tensometrycznej}
  \label{rys:belka_liniowosc}  
\end{figure}

\begin{table}[tp]
  \caption{Parametry belki tensometrycznej}
  \label{tab:belka_liniowosc}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Obciążenie [N]} & {\bf Napięcie wyjściowe $U_{wy+}-U_{wy-}$ [mV]} \\
0 & 0 \\
0.4709 & 0 \\
0.5297 & 0.1000 \\
1.1183 & 0.1000\\
2.0601 & 0.2000\\
3.1196 & 0.3000\\
4.0417 & 0.4000\\
5.5230 & 0.6000\\
7.2300 & 0.8000\\
8.6328 & 0.9000\\
   10.0552 & 1.1000\\
   11.9388 & 1.3000\\
   15.3919 & 1.7000\\
   17.2754 & 1.9000\\
   19.9143 & 2.1000\\
   23.4165 & 2.5000\\
   26.4478 & 2.8000\\
   29.9205 & 3.2000\\
   33.5698 & 3.6000\\
   36.7679 & 3.9000\\
   40.5938 & 4.4000\\
   44.4589 & 4.8000\\
   48.7851 & 5.2000\\
   54.1120 & 5.8000\\
   60.2236 & 6.5000\\
   64.5596 & 7.0000\\
\hline\hline
  \end{tabular}
\end{table}

Charakterystyka wejściowo-wyjściowa belki tensometrycznej z rysunku \ref{rys:belka_liniowosc} prezentuje się bardzo dobrze. Przy 
dość ograniczonej dokładności pomiaru (1 mV) wykres jest liniowy dla wszystkich dozwolonych obciążeń belki, co powinno wpłynąć na
dokładny pomiar siły działającej na poszczególne ramiona manipulatora.
Dodatkowo, w podobny sposób przeprowadzone zostały testy powtarzalności pomiarów. Przy stałym obciążeniu (zerowym, średnim i maksymalnym) 
odłączenie i ponowne podłączenie zasilania belki nie zmieniało napięcia zwracanego na wyjście -- bez wyjątku, przy każdym obciążeniu
pomiary były powtarzalne. Ma to niebagatelne znaczenie przy ocenianiu właściwości czujnika, ponieważ znacząco zwiększa to
prawdopodobieństwo poprawnych odczytów w kolejnych cyklach pracy robota.

\subsection{Umieszczenie czujników na konstrukcji manipulatora}

\chapter{Układ elektroniczny}
Przy sterowaniu korzystającym z aktywnej kontroli siły wymagane jest zastosowanie czujników, które są w stanie
szybko i dokładnie przetwarzać informacje o stanie robota. Ze względu na precyzję i niską cenę najczęściej wykorzystuje
się czujniki elektroniczne. Każdy z takich czujników wymaga doprowadzenia do niego zasilania, a także
odprowadzenia pomiaru do jednostki nadrzędnej.

Poza czujnikami układ elektroniczny składa się także z silników i jednostki sterującej (mikrokontrolera).
Mikrokontroler służy do zbierania pomiarów od czujników, komunikacji z komputerem i generacji sygnałów sterujących podawanych na napędy.
Silniki są elementami wykonawczymi, które pobudzone odpowiednimi napięciami (sterowaniem) generują ruch robota.

\section{Mikrokontroler}\label{sec:mikrokontroler}
Jako jednostkę nadrzędną wykorzystano mikrokontroler \emph{STM32F100RBT6B} \cite{stm32f100} wchodzący w skład
płytki ewaluacyjnej \emph{STM32 value line Discovery} \cite{stmdiscovery}.
Płytka ta (przedstawiona na rysunku \ref{rys:stm32discovery}) wyposażona jest w dużą liczbę dodatkowych elementów,
takich jak diody led, przyciski czy port mini-usb. Co szczególnie istotne, możliwe jest programowanie i zasilenie jej bezpośrednio z portu 
USB, co znacząco ułatwia i przyspiesza proces implementacji. Ponadto, wykorzystując zewnętrzny zasilacz, możliwe jest
uruchomienie płytki przy dwóch poziomach napięcia zasilania -- 5V i 3.3V.

\begin{figure}[tp]
\centering
  \includegraphics[width=0.4\textwidth]{grafika/stm32discovery}
  \caption{Układ STM32 Discovery}
  \label{rys:stm32discovery}  
\end{figure}

Sam mikrokontroler, zawierający 32-bitową jednostką obliczeniową (mikroprocesor), należy do rodziny Cortex wykorzystującej architekturę ARM.
Jest to jedna z najczęściej wykorzystywanych obecnie architektur, głównie ze względu na bardzo dobry stosunek jakości do ceny, moc
obliczeniową i dużą liczbę zewnętrznych peryferiów (takich jak przetworniki analogowo-cyfrowe czy cyfrowe protokoły komunikacyjne).
Najważniejsze parametry mikrokontrolera zebrane zostały w tabeli \ref{tab:mikrokontroler}. Mikroprocesor wchodzący w skład tej jednostki
taktowany jest zegarem 24 Mhz, jednakże konfigurując pętlę PLL w odpowiedni sposób możliwe jest otrzymanie częstotliwości 72 Mhz. 
Dla potrzeb sterowania manipulatora realizowanego w ramach niniejszego projektu jest to w zupełności wystarczające. 

\begin{table}[tp]
  \caption{Parametry mikrokontrolera}
  \label{tab:mikrokontroler}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Parametr} & {\bf Wartość} \\
Taktowanie zegara & 24 Mhz \\
Pamięć Flash & 128 kb \\
Pamięć SRAM & 8 kb \\
Rozdzielczość ADC & 12 bitów\\
Liczba kanałów ADC & 16\\
Rozdzielczość DAC & 12 bitów\\
Liczba kanałów ADC & 16\\
Rozdzielczość timerów & 16 bitów\\
Liczba timerów & 12\\
Protokoły komunikacyjne & I2C, SPI, USART\\
Liczba pinów & 64\\
\hline\hline
  \end{tabular}
\end{table}

\subsection{Programowanie mikrokontrolera}
Do programowania mikrokontrolera została użyta aplikacja \emph{Keil uVision} \cite{keil}, która jest (stale rozwijanym) jednym z najlepszych
narzędzi współpracujących z architekturą ARM. Posiada ona wbudowany debugger, który ułatwia testowanie poprawności
działania programu znajdującego się na mikrokontrolerze. Programowanie układu nie wymaga instalowania dodatkowych sterowników w systemie
i odbywa się za pomocą zwykłego kabla USB. Największą wadą tego środowiska jest kompatybilność jedynie z systemem operacyjnym Windows.

Program uruchamiany w mikrokontrolerze został napisany w języku C. Ma on na celu przetworzenie pomiarów ze wszystkich czujników,
przesłanie danych do jednostki nadrzędnej (komputera) za pomocą protokołu RS232 \ref{sec:komunikacja_z_PC}, a także przetworzenie 
odebranych od komputera danych na sygnały sterujące napędami. Szczegółowy opis implementacji znajduje się w załączniku.

\section{Czujniki}\label{sec:czujniki}

\section{Komunikacja z PC}\label{sec:komunikacja_z_PC}
Komunikacja mikrokontrolera z komputerem została zrealizowana za pomocą asynchronicznego, szeregowego protokołu \emph{USART}, 
wykorzystując standard \emph{RS-232}, który
definiuje normy wtyczek i kabli portów szeregowych typu COM. Parametry transmisji zostały zebrane w tabeli \ref{tab:transmisja}.
Muszą być one ustawione w ten sam sposób zarówno po stronie mikrokontrolera, jak i komputera.

\begin{table}[tp]
  \caption{Parametry transmisji danych portem szeregowym}
  \label{tab:transmisja}
  \centering
  \begin{tabular}{||c|c||}
    \hline\hline
{\bf Parametr} & {\bf Wartość} \\
Prędkość & 9600 baud \\
Parzystość & brak \\
Bity danych & 8 \\
Bity stopu & 1 \\
\hline\hline
  \end{tabular}
\end{table}

Zaimplementowana w języku C++ aplikacja umożliwiająca komunikację z mikrokontrolerem przeznaczona jest do pracy w systemie operacyjnym 
linux, jednakże uruchomienie jej pod systemem Windows jest również możliwe. Przed uruchomieniem programu należy sprawdzić, do którego
portu COM wtyczka jest podłączona (są to przeważnie wartości z przedziału 0-16, w zależności od systemu operacyjnego) i wprowadzić
ją jako zmienną do programu. W momencie, gdy parametry transmisji są ustawione następuje otwarcie portu. Wysyłanie
i odbieranie wiadomości odbywa się w prosty sposób za pomocą spreparowanych do tego funkcji.

Konfiguracja mikrokontrolera w celu komunikacji z komputerem sprowadza się do następujących kroków:
\begin{itemize}
\item włączenie zegara dla peryferiów,
\item ustawienie zegara i kierunkowości portów,
\item włączenie zegara dla USART,
\item skonfigurowanie parametrów transmisji (\ref{tab:transmisja}),
\item włączenie nadajnika i odbiornika w rejestrze CR1,
\item włączenie protokołu szeregowego w rejestrze CR1.
\end{itemize}
Transmisja danych odbywa się poprzez zapis i odczyt bitów z odpowiednich rejestrów. W międzyczasie konieczne jest kontrolowanie bitów odpowiedzialnych
za zakończoną operację, które są ustawiane sprzętowo. Porty, które wykorzystywane są do transmisji danych zawiera tabela \ref{tab:porty_rs232}.

\begin{table}[tp]
  \caption{Porty mikrokontrolera wykorzystane do transmisji szeregowej}
  \label{tab:porty_rs232}
  \centering
  \begin{tabular}{||c|c|c||}
    \hline\hline
{\bf Parametr} & {\bf Nadajnik} & {\bf Odbiornik} \\
Funkcja & USART1\_TX & USART1\_RX \\
Port & A9 & A10\\
\hline\hline
  \end{tabular}
\end{table}

\subsection{Konwersja napięć}
W mikrokontrolerach najczęściej wykorzystywanym standardem jest \emph{TTL}. Jego działanie opiera się na logice dodatniej -- 
sygnał niski (logiczne "0") definiuje się jako napięcie od 0 V do 0,8 V w odniesieniu do masy, a wysoki (logiczna "1") -- od 2 V do 5 V.
Z drugiej strony specyfikacja napięcia standardu RS-232 definiuje jedynkę logiczną jako napięcie od -3V do -15V, zaś zero to napięcie +3V do +15V.
Różnice pomiędzy standardami wprowadzają konieczność zastosowania układu konwersji napięć, który służy do zmiany napięcia
sygnału przesyłanego pomiędzy mikrokontrolerem i komputerem. Najczęściej wykorzystywany jest w tym celu układ scalony \emph{MAX232}, który 
jest dedykowany do tego zastosowania. Schemat skonstruowanego układu znajduje się na rysunku \ref{rys:max232}.

\begin{figure}[tp]
\centering
  \includegraphics[width=0.6\textwidth]{grafika/max232}
  \caption{Układ konwersji napięć pomiędzy standardami TTL i RS-232}
  \label{rys:max232}  
\end{figure}

\section{Silniki}

\chapter{Sterowanie manipulatorem}
Sterowaniem nazywa się proces generowania sygnałów, podawanych na elementy wykonawcze (silniki), na podstawie sygnałów
pobieranych z czujników. Celem sterowania definiowany jest zazwyczaj w oparciu o konkretną funkcjonalność robota.
Do najczęściej wykorzystywanych celów zalicza się wymuszanie zadanej trajektorii ruchu manipulatora, 
utrzymywanie stałej pozycji bądź też
oddziaływanie określoną siłą na obiekty w otoczeniu. Technikę wyznaczania poziomu sygnałów sterujących nazywa się
algorytmem sterowania. Pozycjonowanie i ruch manipulatora w przestrzeni roboczej najczęściej odbywa się poprzez zadawanie prędkości
obrotu silników, jednakże w manipulatorach podatnych brane są pod uwagę także momenty pojawiające się w napędach. W konsekwencji, 
poza standardowym sterowaniem prędkością ruchu robota dodatkowo wprowadza się pojęcie sterowania siłą, polegające na zadawaniu
siły oddziaływania efektora z otoczeniem.

Sterowanie siłą (z ang. \texttt{Force Control}) polega na kontroli sił, którymi robot oddziałuje na otoczenie. Zgodnie z tym, 
co zostało opisane we wstępie (por. \ref{sec:podatnosc}), możliwość kontrolowania stopnia oddziaływania manipulatora na niezależne
obiekty, z którymi jest on w kontakcie, ma cały szereg zastosowań, takich jak chwytanie lub przesuwanie obiektów.

Istnieje wiele metod sterowania siłą, które stosują różne podejścia do tego problemu. Zostały one porównane w podrozdziale
\ref{sec:podatnosc}. W dalszej części tego rozdziału dokładniej opisano jedynie sterowanie hybrydowe
(zaliczające się do kategorii sterowań bezpośrednich), które najlepiej nadaje się do rozpatrywanego manipulatora.

\section{Bezpośrednie sterowanie siłą}
Zastosowanie bezpośredniego podejścia do siłowego sterowania manipulatorem zakłada wykorzystanie pętli sprzężenia zwrotnego od siły \cite{handbook}.
Dane dotyczące wartości i kierunków sił, którymi robot oddziałuje na otoczenie są zbierane z wykorzystaniem czujników, a następnie
wykorzystywane przez algorytm sterowania w celu kompensacji różnicy pomiędzy wartością zadaną, a rzeczywistą.
W zależności od algorytmu sterowanie siłą może odbywać się naprzemiennie bądź też równolegle do sterowania prędkością.
Jednym z rozwiązań jest dekompozycja sterowań siłą i prędkością na oddzielne stopnie swobody, co wpływa na zmniejszenie
przestrzeni roboczej robota, a także ogranicza zestaw jego zastosowań. Z drugiej strony podejście sterowania hybrydowego polega
na generowaniu wspólnego sygnału sterującego, podawanego na silniki, obliczanego na podstawie ważonego błędu położenia (lub prędkości) i siły.

Skrętnikiem nazywa się wektor o rozmiarze n (liczba stopni swobody robota), który składa się z sił i momentów przyłożonych
do ciała sztywnego w konkretnym punkcie. Składowe liniowe (siły) odpowiadają położeniowym stopniom swobody, natomiast
składowe kątowe (momenty) obrotowym. W sterowaniu bezpośrednim za pomocą skrętnika $h_e = (f_e^T m_e^T)T$ opisuje się wektor sił i momentów,
którymi efektor robota oddziałuje na otoczenie. Celem procesu sterowania jest wyznaczenie skrętnika sterującego $h_c$, 
wchodzącego w skład dynamicznego modelu robota będącego w kontakcie z otoczeniem, zdefiniowanego dla przestrzeni zadaniowej:
\begin{equation}
\Lambda(q)\dot{\upsilon}_e + \Gamma(q, \dot{q})\upsilon_e + \eta(q) = h_c - h_e,
\label{eq:dynamika_w_kontakcie}
\end{equation}
gdzie
\begin{itemize}
\item $\upsilon_e = J(q)\dot{q}$ -- prędkość wektora w przestrzeni zadaniowej,
\item $\Lambda(q) = (JH(q)^{-1}J^T)^{-1}$ -- macierz inercji w przestrzeni zadaniowej,
\item $\Gamma(q, \dot{q}) = J^{-T}C(q,\dot{q})J^{-1} - \Lambda(q)\dot{q}J^{-1}$ -- odpowiednik macierzy Coriolisa w przestrzeni zadaniowej,
\item $\eta(q) = J^{-T}g(q)$ -- skrętnik pochodzący od siły grawitacji,
\item $H(q)$ -- macierz inercji manipulatora,
\item $C(q, \dot{q})$ -- macierz Coriolisa,
\item $g(q)$ -- wektor sił grawitacji.
\end{itemize}

Zadane momenty silników manipulatora $\tau$, które wygenerują pożądany skrętnik sterujący można w prosty sposób wyznaczyć z wykorzystaniem Jakobianu
i równania:
\begin{equation}
\tau = J^Th_c.
\label{eq:skretnik_sterujacy}
\end{equation}

Bezpośrednie sterowanie siłą wymaga także pełnej znajomości modelu interakcji manipulatora z otoczeniem, w związku z czym niezbędna
jest początkowa analiza sytuacji kontaktowych.

\subsection{Kontrola interakcji}\label{sec:kontrola_interakcji}
Modelowanie zjawisk występujących przy bezpośredniej interakcja robota z otoczeniem nie należy do zadań trywialnych.
Wpływ na występujące przy kontakcie siły i momenty ma bardzo wiele czynników, takich jak chwilowe przyspieszenie obiektów,
ich masa, gęstość czy sprężystość. Dokładny opis analityczny wszystkich obserwowanych efektów byłby bardzo pożądany, jednakże
rzeczywista interakcja, zarówno lokalnie jak i globalnie, w dużym stopniu zależy od dynamiki robota i środowiska.
Najbardziej skomplikowane zjawiska występujące przy kontakcie robota ze środowiskiem:
\begin{itemize}
\item otoczenie wprowadza ograniczenia na kinematykę robota, których naruszenie sprzyja pojawianiu się niezerowego skrętnika 
(wektora sił i momentów) zarówno po stronie manipulatora, jak i środowiska,
\item strukturalna podatność robota ma wpływ na wariacje pojawiających się sił i momentów, które są konsekwencją skończonej sztywności
przegubów i ogniw,
\item lokalne deformacje powierzchni elementów będących w kontakcie, którym w skutek zużycia zmieniają parametry fizyczne,
\item brak powierzchni idealnie gładkich implikuje występowanie statycznego i dynamicznego tarcia, które jest trudne do zamodelowania.
\end{itemize}

W związku z tym, że wygenerowanie bezbłędnego modelu jest operacją niewykonalną, wprowadza się kilka założeń, które
pozwalają uprościć model interakcji. Wyróżniane są dwa podstawowe podejścia:

\begin{itemize}
\item robot i elementy środowiska są ciałami idealnie sztywnymi, a ograniczenia na kinematykę wynikają bezpośrednio z konfiguracji otoczenia,
\item robot jest ciałem idealnie sztywnym, natomiast podatność otoczenia aproksymowana jest przez liniowy model.
\end{itemize}

W obu przypadkach zakłada się także brak tarcia w sytuacjach kontaktowych. W dalszych rozważaniach pod uwagę będzie brane
jedynie pierwsze z rozwiązań, ponieważ modelowanie podatności otoczenia jest zadaniem bardziej skomplikowanym,
głównie ze względu na potrzebę generowania i uwzględniania w obliczeniach dodatkowej macierzy podatności.

\subsection{Hybrydowanie sterowanie prędkością/siłą}
Sterowanie hybrydowe umożliwia jednoczesną kontrolę zarówno prędkości, jak i siły którą manipulator oddziałuje na otoczenie. 
W przeciwieństwie do pozostałych metod bezpośrednich nie wymaga ono rozdzielania tych zadań na oddzielne stopnie swobody,
dzięki czemu możliwe jest sterowanie prędkością i siłą w jednym kierunku. Zastosowanie sterowania hybrydowego pozwala na
wprowadzenie w manipulatorze aktywnej podatności poprzez analizę pojawiających się zewnętrznych sił w trakcie ruchu robota.

Wyróżnia się trzy podstawowe podejścia do sterowania hybrydowego:
\begin{itemize}
\item w oparciu o pasywność dynamiki,
\item w oparciu o prędkości,
\item w oparciu o przyspieszenia.
\end{itemize}

Sterowanie hybrydowe w oparciu o pasywność korzysta ze skośnej symetrii modelu dynamiki robota. Właściwość ta jest
wykorzystywana bezpośredni przy obliczaniu skrętnika sterującego, przez co wykorzystanie tej metody bez znajomości dynamiki jest niemożliwe.
Podejście prędkościowe jest stosunkowo najprostszym z rozwiązań, ponieważ nie wymaga ono modelu dynamiki, a sterowanie
odbywa się w oparciu jedynie o zadaną prędkość (nie ma potrzeby zadawania przyspieszeń). Niestety metoda ta może być
wykorzystana wyłącznie dla interakcji wystarczająco podatnych, w których nie występują duże przyspieszenia
przy kontakcie dwóch idealnie sztywnych ciał (jest to jednym z przyjętych założeń, rozdział \ref{sec:kontrola_interakcji}).

Podejście do sterowania hybrydowego korzystając z przyspieszeń opiera się na odpsrzęganiu i linearyzacji nieliniowej dynamiki
robota. Przy kontakcie robota z otoczeniem odsprzęganie pomiędzy prędkością i siłą jest zjawiskiem bardzo pożądanym. 
W związku z tym konstruuje się dwie pętle sterowania, wewnętrzną i zewnętrzną. Pierwsza z nich
odpowiada za kompensację nieliniowej dynamiki robota i rozdzielenie przestrzeni prędkości od przestrzeni sił. 
Druga natomiast służy do niwelowania błędów w śledzeniu zadanego ruchu i oddziaływań efektora z otoczeniem.

Przy założeniu idealnej sztywności ciał w środowisku zewnętrzny skrętnik, którym robot oddziałuje na otoczenie zapisuje się
jako $h_e = S_f \lambda$, gdzie $\lambda$ jest wektorem sił zewnętrznych, natomiast $S_f$ jest macierzą przekształcającą
go do przestrzeni zadaniowej manipulatora. -- Czy to jest dobrze?
Wektor $\lambda$ zależy od skrętnika sterującego $h_c$, w związku z czym poprzez odpowiedni dobór jego wartości
możliwe jest kontrolowanie oddziaływania robota na otoczenie. Dla rozpatrywanego podejścia pożądany skrętnik wyznacza się ze wzoru:
\begin{equation}
h_c = \Lambda(q)S_v\alpha_v + S_ff_{\lambda} + \mu(q, \dot{q}) + \Lambda(q)\dot{S}_v\upsilon,
\end{equation}
gdzie $\alpha_v$ i $f_{\lambda}$ są odpowiednio dobranymi wejściami sterującymi, $\mu(q, \dot{q}) = \Gamma \dot{q} + \eta$, natomiast $S_v$
jest macierzą transformacji wektora prędkości. 

Wejścia sterujące wykorzystywane są jako druga pętla sprzężenia zwrotnego, której celem jest śledzenie zadanych sił i prędkości. 
Parametr $f_{\lambda}$ służy do obliczenia sygnału sterującego na podstawie błędu siły definiowanego jako siłę zadaną i rzeczywistą
pobraną z czujników, przemnożonego przez odpowiednią, pozytywnie zdefiniowaną macierz K (analogicznie do regulatorów PID). 
Dwa przypadki zdefiniowane w książce \cite{handbook} zawierają równania \eqref{eq:flambda1} i \eqref{eq:flambda2}.
\begin{equation}
f_{\lambda} = \lambda_d(t) + K_{P\lambda}[\lambda_d(t) - \lambda(t)],
\label{eq:flambda1}
\end{equation}

\begin{equation}
f_{\lambda} = \lambda_d(t) + K_{I\lambda} \int_0^t \! [\lambda_d(\tau) - \lambda(\tau)] \, \mathrm{d} \tau,
\label{eq:flambda2}
\end{equation}

Definicję parametru $\alpha_v$, służącego do sterowania prędkością, zawiera równanie \eqref{eq:alphav1}. Możliwe jest także
przekształcenie rozpatrywanego zadania do równoległego sterowania siłą i pozycją (co zostanie wykorzystane w rozpatrywanym manipulatorze).
W takim przypadku parametr $\alpha_v$ definiuje się zgodnie z równaniem \eqref{eq:alphav2}.
\begin{equation}
\alpha_v = \dot{\upsilon}_d(t) + K_{P\upsilon}[\upsilon_d(t) - \upsilon(t)] + 
K_{I\upsilon} \int_0^t \! [\upsilon_d(\tau) - \upsilon(\tau)] \, \mathrm{d} \tau.
\label{eq:alphav1}
\end{equation}

\begin{equation}
\alpha_v = \ddot{r}_d(t) + K_{Dr}[\dot{r}_d(t) - \upsilon(t)] + 
K_{Pr}[\dot{r}_d(t) - r(t)].
\label{eq:alphav2}
\end{equation}

Dla sterowania położeniem zdefiniowanego równaniem \eqref{eq:alphav2} konieczne jest obliczenie drugiej pochodnej położenia,
co wymusza definiowanie dostatecznie gładkiej trajektorii zadanej.

\section{Algorytm sterowania}

%\chapter{Podsumowanie projektu}



\appendix
\chapter{Jakobian manipulatora}\label{sec:dodatek_jakobian_manipulatora}
Poszczególne elementy jakobianu manipulatora zostały wyszczególnione poniżej. Ze względu na złożoność równań,
wprowadzono szereg podstawień:\\
\\
$
s_{a1} = \sin(q_{a1})\\
c_{a1} = \cos(q_{a1})\\
s_{a2} = \sin(q_{a2})\\
c_{a2} = \cos(q_{a2})\\
s_{b1} = \sin(q_{b1})\\
c_{b1} = \cos(q_{b1})\\
s_{b2} = \sin(q_{b2})\\
c_{b2} = \cos(q_{b2})\\
s_{aa} = \sin(q_{a1}+q_{a2})\\
c_{aa} = \cos(q_{a1}+q_{a2})\\
s_{bb} = \sin(q_{b1}+q_{b2})\\
c_{bb} = \cos(q_{b1}+q_{b2})\\
s_{ab} = \sin(q_{a1}-q_{b1})\\
c_{ab} = \cos(q_{a1}-q_{b1})\\
s_{aab} = \sin(q_{a1}+q_{a2}-q_{b1})\\
c_{aab} = \cos(q_{a1}+q_{a2}-q_{b1})\\
s_{abb} = \sin(q_{a1}-q_{b1}-q_{b2})\\
c_{abb} = \cos(q_{a1}-q_{b1}-q_{b2})\\
s_{aabb} = \sin(q_{a1}+q_{a2}-q_{b1}-q_{b2})\\
c_{aabb} = \cos(q_{a1}+q_{a2}-q_{b1}-q_{b2})\\
x_a = l_0 + l_1 \cos(q_{a1}) + l_2 \cos(q_{a1}+q_{a2})\\
x_b = -l_0 + l_1 \cos(q_{b1}) + l_2 \cos(q_{b1}+q_{b2})\\
y_a = l_1 \sin(q_{a1}) + l_2 \sin(q_{a1}+q_{a2})\\
y_b = l_1 \sin(q_{b1}) + l_2 \sin(q_{b1}+q_{b2})
$
\\\\
Elementy jakobianu zadane są wzorami:\\
\begin{multline*}
J_{11} = 
\frac{l_4 (y_a - y_b)^2 ((x_a - l_0) (x_a - x_b)+(y_a) (y_a - y_b))}{(x_a - x_b)^4 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
-\frac{l_4 (x_a - l_0)}{(x_a - x_b) \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}-\frac{l_4 (y_a) (y_a - y_b)}{(x_a - x_b)^2 \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}+\\
-l_3\left(\frac{(x_a - l_0) (x_a - x_b)+(y_a) (y_a - y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)} -
\frac{-2 l_0 y_a+l_1^2 s_{ab}+l_1 l_2 s_{aab}+l_1 l_2 s_{abb}+l_2^2 s_{aabb}}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\right) \cdot\\ \cdot \sin\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] 
\end{multline*}

\begin{multline*}
J_{12} = 
 -\frac{l_2 l_4 (-l_2-l_1 c_{a2}-2 l_0 c_{aa}+l_1 c_{aab}+l_2 c_{aabb}) (y_a - y_b)^2}{(x_a - x_b)^4 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
 -\frac{l_2 l_4 c_{aa}}{(x_a - x_b) \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}-\frac{l_2 l_4 s_{aa} (y_a - y_b)}{(x_a - x_b)^2 \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}+
 \\
 -l_3 \langle-\frac{l_2 (-l_2-l_1 c_{a2}-2 l_0 c_{aa}+l_1 c_{aab}+l_2 c_{aabb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 x_a+l_1 l_2 c_{a2}-l_1^2 c_{ab}-l_1 l_2 c_{aab}-2 l_0 l_1 x_b-l_1 l_2 c_{abb}-l_2^2 c_{aabb}+l_1 l_2 c_{b2}\right)}
 \\
 -\frac{l_2 (-l_1 s_{a2}-2 l_0 s_{aa}+l_1 s_{aab}+l_2 s_{aabb})}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle \cdot 
 \\ \cdot \sin\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] 
\end{multline*}

\begin{multline*}
J_{13} = 
 -l_1 s_{b1}-l_2 s_{bb}+\\
 +\frac{l_4 \left(-l_1^2-l_2^2+l_1^2 c_{ab}+l_1 l_2 c_{aab}+2 l_0 l_1 c_{b1}+l_1 l_2 c_{abb}+l_2^2 c_{aabb}-2 l_1 l_2 c_{b2}+2 l_0 l_2 c_{bb}\right) (y_a - y_b) (-y_a+y_b)}{(x_a - x_b)^4 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
 -\frac{l_4 (x_b - l_0)}{(-2 l_0-l_1 c_{a1}-l_2 c_{aa}+x_b - l_0) \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}+\frac{l_4 (y_a - y_b) (y_b)}{(x_a - x_b)^2 \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}+
 \\
 -l_3 \langle\frac{(x_b - l_0) (-2 l_0-l_1 c_{a1}-l_2 c_{aa}+x_b - l_0)+(y_b) (-y_a+y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)}+\\
 +\frac{l_1^2 s_{ab}+l_1 l_2 s_{aab}-2 l_0 l_1 s_{b1}+l_1 l_2 s_{abb}+l_2^2 s_{aabb}-2 l_0 l_2 s_{bb}}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle \cdot 
 \\
 \cdot \sin\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] 
\end{multline*}

\begin{multline*}
J_{14} = 
 -l_2 s_{bb}+\frac{l_2 l_4 (-l_2+l_1 c_{abb}+l_2 c_{aabb}-l_1 c_{b2}+2 l_0 c_{bb}) (y_a - y_b) (-y_a+y_b)}{(x_a - x_b)^4 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
 +\frac{l_2 l_4 c_{bb}}{(x_a - x_b) \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}} +\frac{l_2 l_4 s_{bb} (y_a - y_b)}{(x_a - x_b)^2 \sqrt{1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}}}
 \\
 -l_3 \langle\frac{-l_2 (-l_2+l_1 c_{abb}+l_2 c_{aabb}-l_1 c_{b2}+2 l_0 c_{bb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 l_1 (c_{a1}-c_{b1})+l_1 l_2 (c_{a2} + c_{b2} - c_{aab} - c_{abb} )+2 l_0 l_2 (c_{aa}-c_{bb})-l_1^2 c_{ab}-l_2^2 c_{aabb}\right)}+
 \\
 +\frac{l_2 (l_1 s_{abb}+l_2 s_{aabb}+l_1 s_{b2}-2 l_0 s_{bb})}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle \cdot 
 \\
 \cdot \sin\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right]
\end{multline*}

\begin{multline*}
J_{21} = 
 -\frac{l_4 (y_a - y_b) ((x_a - l_0) (x_a - x_b)+(y_a) (y_a - y_b))}{(x_a - x_b)^3 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
 +l_3 \cos\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] \cdot
 \\
 \cdot \left(\frac{(x_a - l_0) (x_a - x_b)+(y_a) (y_a - y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)}-\frac{-2 l_0 l_1 s_{a1}-2 l_0 l_2 s_{aa}+l_1^2 s_{ab}+l_1 l_2 s_{aab}+l_1 l_2 s_{abb}+l_2^2 s_{aabb}}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\right) 
\end{multline*}

\begin{multline*}
J_{22} = 
 \frac{l_2 l_4 (-l_2-l_1 c_{a2}-2 l_0 c_{aa}+l_1 c_{aab}+l_2 c_{aabb}) (y_a - y_b)}{(x_a - x_b)^3 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+\\
 +l_3 \cos\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] \cdot
 \\
 \cdot \langle-\frac{l_2 (-l_2-l_1 c_{a2}-2 l_0 c_{aa}+l_1 c_{aab}+l_2 c_{aabb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 l_1 (c_{a1}-c_{b1})+l_1 l_2 (c_{a2} + c_{b2} - c_{aab}-c_{abb})+2 l_0 l_2 (c_{aa}-c_{bb})-l_1^2 c_{ab}-l_2^2 c_{aabb}\right)}+
 \\
 -\frac{l_2 (-l_1 s_{a2}-2 l_0 s_{aa}+l_1 s_{aab}+l_2 s_{aabb})}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle
\end{multline*}

\begin{multline*}
J_{23} = 
 x_b - l_0+\\
 -\frac{l_4 \left(-l_1^2-l_2^2+l_1^2 c_{ab}+l_1 l_2 c_{aab}+2 l_0 l_1 c_{b1}+l_1 l_2 c_{abb}+l_2^2 c_{aabb}-2 l_1 l_2 c_{b2}+2 l_0 l_2 c_{bb}\right) (-y_a+y_b)}{(x_a - x_b)^3 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+
 \\
 +l_3 \cos\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] \cdot
 \\
 \cdot \langle\frac{(x_b - l_0) (-2 l_0-l_1 c_{a1}-l_2 c_{aa}+x_b - l_0)+(y_b) (-y_a+y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)}+\\
 +\frac{l_1^2 s_{ab}+l_1 l_2 s_{aab}-2 l_0 l_1 s_{b1}+l_1 l_2 s_{abb}+l_2^2 s_{aabb}-2 l_0 l_2 s_{bb}}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle 
\end{multline*}

\begin{multline*}
J_{24} = 
 l_2 c_{bb}-\frac{l_2 l_4 (-l_2+l_1 c_{abb}+l_2 c_{aabb}-l_1 c_{b2}+2 l_0 c_{bb}) (-y_a+y_b)}{(x_a - x_b)^3 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)^{3/2}}+
 \\
 +l_3 \cos\left[\text{arcsec}\left[\frac{2 l_3}{\sqrt{(x_a - x_b)^2+(y_a - y_b)^2}}\right]+\arctan\left[\frac{y_a - y_b}{x_a - x_b}\right]\right] \cdot
 \\
 \cdot \langle-\frac{l_2 (-l_2+l_1 c_{abb}+l_2 c_{aabb}-l_1 c_{b2}+2 l_0 c_{bb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 l_1 (c_{a1}-c_{b1})+l_1 l_2 (c_{a2}+c_{b2}-c_{aab}-c_{abb})+2 l_0 l_2 (c_{aa}-c_{bb})-l_1^2 c_{ab}-l_2^2 c_{aabb}\right)}+
 \\
 +\frac{l_2 (l_1 s_{abb}+l_2 s_{aabb}+l_1 s_{b2}-2 l_0 s_{bb})}{2 l_3 \sqrt{(x_a - x_b)^2+(y_a - y_b)^2} \sqrt{1-\frac{(x_a - x_b)^2+(y_a - y_b)^2}{4 l_3^2}}}\rangle
\end{multline*}

\begin{equation*}
J_{31} = 
 \frac{(x_a - l_0) (x_a - x_b)+(y_a) (y_a - y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)} 
\end{equation*}

\begin{equation*}
J_{32} = 
 -\frac{l_2 (-l_2-l_1 c_{a2}-2 l_0 c_{aa}+l_1 c_{aab}+l_2 c_{aabb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 l_1 (c_{a1}-c_{b1})+l_1 l_2 (c_{a2}+c_{b2}-c_{aab}-c_{abb})+2 l_0 l_2 (c_{aa}-c_{bb})-l_1^2 c_{ab}-l_2^2 c_{aabb}\right)} 
\end{equation*}

\begin{equation*}
J_{33} = 
 \frac{(x_b - l_0) (-2 l_0-l_1 c_{a1}-l_2 c_{aa}+x_b - l_0)+(y_b) (-y_a+y_b)}{(x_a - x_b)^2 \left(1+\frac{(y_a - y_b)^2}{(x_a - x_b)^2}\right)} 
\end{equation*}

\begin{equation*}
J_{34} = 
 -\frac{l_2 (-l_2+l_1 c_{abb}+l_2 c_{aabb}-l_1 c_{b2}+2 l_0 c_{bb})}{2 \left(2 l_0^2+l_1^2+l_2^2+2 l_0 l_1 (c_{a1}-c_{b1})+l_1 l_2 (c_{a2}+c_{b2}-c_{aab}-c_{abb})+2 l_0 l_2 (c_{aa}-c_{bb})-l_1^2 c_{ab}-l_2^2 c_{aabb}\right)}
\end{equation*}


%\chapter{Zawartość płyty CD}
%Do pracy dołączono płytę CD zawierającą:
%\begin{enumerate}
%\item Aplikacja -- katalog zawierający kod źródłowy aplikacji,
%\item Doc -- katalog zawierający dokumentację kodu źródłowego przy wykorzystaniu środowiska Doxygen
%\item KDL -- katalog zawierający kod źródłowy biblioteki KDL \cite{KDL}
%\item Projekt\_Inzynierski.pdf -- wersja elektroniczna niniejszego dokumentu
%\end{enumerate}

\addcontentsline{toc}{chapter}{Bibliografia} %utworzenie w spisie treści pozycji Bibliografia
%bibliografia
\bibliography{bibliografia} % wstawia bibliografię korzystając z pliku bibliografia.bib - dotyczy BibTeXa, jeżeli nie korzystamy z BibTeXa należy użyć otoczenia thebibliography




%opcjonalnie może się tu pojawić spis rysunków i tabel
% \listoffigures
% \listoftables


\end{document}

